/****************************************************************************/ /* Copyright 1990 to 1995 MBARI */ /****************************************************************************/ /* Summary : Camera Microcontroller Software */ /* Filename : camera.c */ /* Author : Douglas Au */ /* Project : Tiburon ROV */ /* Version : 1.0 */ /* Created : 02/23/95 */ /* Modified : 02/23/95 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: /usr/tiburon/.cvsroot/micro/camera/camera.c,v 1.2 1997/05/07 15:57:32 pean Exp $ * $Log: camera.c,v $ * Revision 1.2 1997/05/07 15:57:32 pean * Cleaned up Include files and makefile for new directory structure. * * Revision 1.1.1.1 1997/05/02 19:17:18 pean * Initial check in of the camera microcontroller software * * Initial revision * */ /****************************************************************************/ #pragma model(196) #pragma nosignedchar #include "C:/C96/include/80C196.h" /* 80196 Register mapping */ #include "C:/C96/include/stddef.h" /* C Standard Definitions */ #include "C:/C96/include/stdlib.h" /* C Standard Library Definitions */ #include "C:/C96/include/stdarg.h" /* C var args Definitions */ #include "const.h" /* Misc. constants - TRUE/FALSE etc */ #include "types.h" /* MBARI style guide declarations */ #include "microasm.h" /* Assembly language functions */ #include "malloc.h" /* Malloc support routines */ #include "ring.h" /* Ring buffer support routines */ #include "strlib.h" /* My string library functions */ #include "microdef.h" /* Microcontroller Definitions */ #include "microcmd.h" /* microcontroller commands */ #include "microlib.h" /* Microcontroller Library decls. */ #include "applay.h" /* Application layer funcitions */ #include "applic.h" /* Microcontroller applications */ #include "serial.h" #include "timer.h" /* Timer library definitions */ #include "../ibc/filter/filter.h" /* General Filter Routines */ #include "../hardware/micro/syslib.h" /* Hardware support library decls */ #include "cam.h" /* Application Specific Definitions */ #include "camboard.h" /* Camera Daughter board Defs */ #define TEMPERATURE_PERIOD 10000 /* Temperature Sample Interval msecs */ #define limit(x, lim) (x > lim ? x = lim : (x < -lim ? x = -lim : 0)) const Char micro_id[] = "87C196KD Microcontroller Board"; const Char softwareRev[] = "$Revision: 1.2 $"; const Char serialNumber[] = "CAMERA 01"; const Char *microSerialNo; #define ATOD_BUF_SIZE 2048 MBool cameraLink; MBool whiteBalFlag; MLocal MBool colorBarsMode; MLocal MBool menuMode; MLocal MBool pageState; MLocal MBool itemState; MLocal MBool upState; MLocal MBool downState; MLocal MBool irisMode; MLocal Int16 irisValue; MLocal Int16 gainMode; MLocal Int16 gainValue; MLocal Int16 detailMode; MLocal Int16 shutterSpeed; MLocal Int16 sceneFile; MLocal Int16 whiteBalMode; MLocal Int16 autoWhiteBalMode; MLocal Int16 totalPedstal; MLocal Int16 redPedstal; MLocal Int16 bluePedstal; MLocal Int16 redGain; MLocal Int16 blueGain; Int16 atodBuf[ATOD_BUF_SIZE]; /* Global Variables Passed into Config.s*/ Byte port1Image; /* IOPORT1 Bit pattern */ Word zoomMotorOffTime; /* PWM 0 on Time in Timer1 ticks */ Word focusMotorOffTime; /* PWM 1 on Time in Timer1 ticks */ Word pwmRate; /* PWM Rate */ Word totalPwmRate; /* Total PWM Rate */ /* servo system parameters */ MLocal Int16 zoomRate; MLocal Int16 zoomInput; MLocal Int16 setPoint[NUMBER_OF_SERVOS]; /* current set point */ MLocal Int16 prevSetPoint[NUMBER_OF_SERVOS]; /* previous set point */ MLocal Int16 position[NUMBER_OF_SERVOS]; /* current position */ MLocal Int16 prevPosition[NUMBER_OF_SERVOS]; /* previous position */ MLocal Int16 prevError[NUMBER_OF_SERVOS]; /* previous error */ MLocal Int16 integError[NUMBER_OF_SERVOS];/* integrated error */ MLocal Int16 tachometer; /* tachometer on zoom motor */ /* Pot and tach zero offsets */ MLocal Int16 positionOffset[NUMBER_OF_SERVOS]; MLocal Int16 tachOffset; /* tach reading at zero velocity */ MLocal Int16 velocityGain[NUMBER_OF_SERVOS]; /* Rate or Position Mode Flag */ MLocal Int16 controlMode[NUMBER_OF_SERVOS]; /* The Servo Motion Command */ MLocal Int16 servoCommand[NUMBER_OF_SERVOS]; /*****************************New Control Law Coefficients*******************/ /* Two sets of constants for each controller - one for position mode and one for rate mode. */ MLocal Int16 propK[NUMBER_OF_SERVOS][2]; /* proportional term */ MLocal Int16 integK[NUMBER_OF_SERVOS][2]; /* integral term */ MLocal Int16 diffK[NUMBER_OF_SERVOS][2]; /* differential term */ MLocal Boolean microEnabled; /* Enables processing of serial Commands*/ MLocal alarmParam tempAlarm; /* Temperature Alarm Monitor */ MLocal alarmParam humidityAlarm; /* Humidity Alarm Monitor */ MLocal filterParam tempFilter; /* Temperature filter */ MLocal filterParam focusFilter; /* Focus filter TESTING ONLY pean */ MLocal filterParam servoFilter; /* Servo filter */ Boolean waterAlarmDetected; /* Water Alarm Detected Flag */ /* Forward Declarations for Locals */ MLocal Void executeCommand( Void ); MLocal Void checkHdWaterAlarm( Void ); MLocal Void checkHumidityAlarm( Void ); MLocal Void checkTempAlarm( Void ); MLocal Void initAlarms( Void ); /* Servo Micro Commands */ MLocal Void setFocusSetPoint(Byte *commandBuf); MLocal Void getFocusSetPoint( Void ); MLocal Void getFocusPosition( Void ); MLocal Void setFocusVelocityGain(Byte *commandBuf); MLocal Void getFocusVelocityGain( Void ); MLocal Void setFocusControlMode(Byte *commandBuf); MLocal Void getFocusControlMode( Void ); MLocal Void setZoomSetPoint(Byte *commandBuf); MLocal Void getZoomSetPoint( Void ); MLocal Void getZoomPosition( Void ); MLocal Void setZoomVelocityGain(Byte *commandBuf); MLocal Void getZoomVelocityGain( Void ); MLocal Void setZoomControlMode(Byte *commandBuf); MLocal Void getZoomControlMode( Void ); MLocal Void setZoomRate(Byte *commandBuf); MLocal Void getZoomRate( Void ); MLocal Void getPWMRate( Void ); MLocal Void setPWMRate(Byte *commandBuf); /* Camera Response Messages */ Byte data_resp[DATA_RESP_NUM][DATA_RESP_SIZE]; /* Camera Control Commands */ MLocal Void setColorBarsModeCmd(Byte *commandBuf); MLocal Void getColorBarsModeCmd( Void ); MLocal Void setColorBars(Boolean state); MLocal Void setMenuModeCmd(Byte *commandBuf); MLocal Void getMenuModeCmd( Void ); MLocal Void setMenuMode(Boolean state); MLocal Void setPageStateCmd(Byte *commandBuf); MLocal Void getPageStateCmd( Void ); MLocal Void setPageState(Boolean state); MLocal Void setItemStateCmd(Byte *commandBuf); MLocal Void getItemStateCmd( Void ); MLocal Void setItemState(Boolean state); MLocal Void setUpStateCmd(Byte *commandBuf); MLocal Void getUpStateCmd( Void ); MLocal Void setUpState(Boolean state); MLocal Void setDownStateCmd(Byte *commandBuf); MLocal Void getDownStateCmd( Void ); MLocal Void setDownState(Boolean state); MLocal Void setIrisModeCmd(Byte *commandBuf); MLocal Void getIrisModeCmd( Void ); MLocal Void setIrisMode(Boolean state); MLocal Void setIrisValueCmd(Byte *commandBuf); MLocal Void getIrisValueCmd( Void ); MLocal Void setIrisValue(Int16 value); MLocal Void setGainModeCmd(Byte *commandBuf); MLocal Void getGainModeCmd( Void ); MLocal Void setGainMode(Int16 state); MLocal Void setGainValueCmd(Byte *commandBuf); MLocal Void getGainValueCmd( Void ); MLocal Void setGainValue(Int16 value); MLocal Void setDetailModeCmd(Byte *commandBuf); MLocal Void getDetailModeCmd( Void ); MLocal Void setDetailMode(Int16 state); MLocal Void setShutterSpeedCmd(Byte *commandBuf); MLocal Void getShutterSpeedCmd( Void ); MLocal Void setShutterSpeed(Int16 state); MLocal Void setSceneFileCmd(Byte *commandBuf); MLocal Void getSceneFileCmd( Void ); MLocal Void setSceneFile(Int16 state); MLocal Void setWhiteBalModeCmd(Byte *commandBuf); MLocal Void getWhiteBalModeCmd( Void ); MLocal Void setWhiteBalMode(Int16 state); MLocal Void setAutoWhiteBalModeCmd(Byte *commandBuf); MLocal Void getAutoWhiteBalModeCmd( Void ); MLocal Void setAutoWhiteBalMode(Int16 state); MLocal Void setTotalPedstalCmd(Byte *commandBuf); MLocal Void getTotalPedstalCmd( Void ); MLocal Void setTotalPedstal(Int16 state); MLocal Void setRedPedstalCmd(Byte *commandBuf); MLocal Void getRedPedstalCmd( Void ); MLocal Void setRedPedstal(Int16 state); MLocal Void setBluePedstalCmd(Byte *commandBuf); MLocal Void getBluePedstalCmd( Void ); MLocal Void setBluePedstal(Int16 state); MLocal Void setRedGainCmd(Byte *commandBuf); MLocal Void getRedGainCmd( Void ); MLocal Void setRedGain(Int16 state); MLocal Void setBlueGainCmd(Byte *commandBuf); MLocal Void getBlueGainCmd( Void ); MLocal Void setBlueGain(Int16 state); /* Servo Control Functions */ MLocal Void servo( Void ); MLocal Void servoAction( Int16 controller ); MLocal Void zoomOutput( Int16 command ); MLocal Void focusOutput( Int16 command ); MLocal Void controlLaw( Int16 controller ); MLocal Void stallCheck( Int16 controller ); MLocal Void measure( Int16 controller ); MLocal Void readTachOffset( Void ); MLocal Void initServos( Void ); MLocal Void zeroServos( Void ); MLocal Void initOffset(Int16 controller); MLocal Int16 scaleMultiply(Int16 a, Int16 b); MLocal Void initDataResp( Void ); MLocal Void checkCameraLink( Void ); Void setPort2Bit( Void ); Void clrPort2Bit( Void ); Extern Void start_pwm( Void ); /* External Function Starts PWM outputs */ /****************************************************************************/ /* Function : processSRQPacket */ /* Purpose : proceses Service Request Packets */ /* Inputs : received packet buffer */ /* Outputs : None */ /****************************************************************************/ Void processSRQPacket(Byte *command_buf) { } /* processSRQPacket() */ /****************************************************************************/ /* Function : initAlarms */ /* Purpose : Initializes various alarm status and thresholds */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void initAlarms( Void ) { tempAlarm.value = 0; tempAlarm.warnThresh = TEMP_WARN_THRESH; tempAlarm.alarmThresh = TEMP_ALARM_THRESH; tempAlarm.alarmStatus = MICRO_ALARM_OK; humidityAlarm.value = 0; humidityAlarm.warnThresh = HUMIDITY_WARN_THRESH; humidityAlarm.alarmThresh = HUMIDITY_ALARM_THRESH; humidityAlarm.alarmStatus = MICRO_ALARM_OK; waterAlarmDetected = FALSE; /* Clear water alarm status flags */ } /* initAlarms() */ /****************************************************************************/ /* Function : mainLoop */ /* Purpose : C main program called from assembler startup routine */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ Void mainLoop( Void ) { initIoport1(); /* Initialize Input/Output Port 1 */ initIoport2(); /* Initialize Input/Output Port 2 */ initMalloc(); /* Initialize Memory allocator */ initBoardStatus(); /* Initialize board status word */ microSerialNo = serialNumber; initAlarms(); /* Initialize application specific alarms */ microEnabled = FALSE; /* Disabled processing of serial commands */ initSerialProtocol(NULL); /* Initialize serial protocol */ /* Initialize temperature filter */ initIIRFilter(&tempFilter, MAX_TEMP_VALUE); initIIRFilter(&focusFilter, 1100); initIIRFilter(&servoFilter, 1100); /* Send Micro Reset SRQ */ writeServiceRequest(1, MICRO_RESET_SRQ); /* Set up Default Camera Settings */ initDataResp(); cameraLink = CAMERA_LINK_DOWN; whiteBalFlag = FALSE; camUartInit(UART_ADDRESS); /* Initialize Camera UART */ camUartOpenSerialChan(CAM_SERIAL_CHAN, RS232_MODE); initServos(); /* Initialize Lens servos */ /* Send Application Initialized SRQ */ writeServiceRequest(1, INITIALIZED_SRQ); FOREVER { /* Repeat forever */ executeCommand(); /* Process any commands from the VME CPU */ checkCameraLink(); /*setPort2Bit();*/ servo(); /*clrPort2Bit();*/ checkHdWaterAlarm(); /* Check water alarms */ checkHumidityAlarm(); /* Check humidity alarm */ checkTempAlarm(); /* Check temperature alarm */ } /* FOREVER */ } /* mainLoop */ /****************************************************************************/ /* Function : microGetTemperature */ /* Purpose : Send temperature value to serial port */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void microGetTemperature( Void ) { /* Send temperature to serial port */ replyToCommand(2, CMD_OK, tempAlarm.value); } /* microGetTemperature() */ /****************************************************************************/ /* Function : microGetTempAlarm */ /* Purpose : Process command to read temperature alarm status */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void microGetTempAlarm( Void ) { replyToCommand(2, CMD_OK, tempAlarm.alarmStatus); /* send temperature alarm status */ /* to the serial port */ } /* microGetTempAlarm() */ /****************************************************************************/ /* Function : microGetTempThresh */ /* Purpose : Send temeprature alarm threshold to serial port */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void microGetTempThresh( Void ) { /* send temp alarm thresh to serial port*/ replyToCommand(3, CMD_OK, tempAlarm.warnThresh, tempAlarm.alarmThresh); } /* microGetTempThresh() */ /****************************************************************************/ /* Function : microSetTempThresh */ /* Purpose : Process command to set temperature alarm thresholds */ /* Inputs : Serial command buffer */ /* Outputs : None */ /****************************************************************************/ MLocal Void microSetTempThresh(Byte *commandBuf) { Word command; /* Serial command word - ignored */ /* Read threshold from command buf */ wordsFromBuf(commandBuf, 3, &command, (Word*) &tempAlarm.warnThresh, (Word*) &tempAlarm.alarmThresh); replyToCommand(1, CMD_OK); /* Send command OK */ } /* microSetTempThresh() */ /****************************************************************************/ /* Function : readTemperature */ /* Purpose : Read temperature sensor */ /* Inputs : None */ /* Outputs : Return temperature sensor reading in degrees C */ /****************************************************************************/ MLocal Int16 readTemperature( Int16 channel ) { /* Assuming a total of 3.24K Resistor Value */ Int16 raw_temp; /* Raw temperature from sensor */ Int16 degrees; /* Converted to degrees Centigrade */ /* 4.9mv per count, 1 counts per 1.5 */ /* deg. kelvin */ /* 3.3mv per Degree Kelvin */ /* Read raw value from sensor */ raw_temp = readAnalogChannel(channel); /* subtract offset and multiply by */ /* 10 to improve dynamic range for */ /* the conversion to degrees */ degrees = (( (raw_temp * 30) / 20) - 273); return(degrees); /* Return temperature in degrees C */ } /* readTemperature() */ /****************************************************************************/ /* Function : getHumidity */ /* Purpose : Send humidity sensor value to serial port */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void getHumidityCmd( Void ) { /* send humidity to serial port */ replyToCommand(2, CMD_OK, humidityAlarm.value); } /* getHumidityCmd() */ /****************************************************************************/ /* Function : getHumidityAlarm */ /* Purpose : Process command to read humidity alarm status */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void getHumidityAlarmCmd( Void ) { replyToCommand(2, CMD_OK, (Word) humidityAlarm.alarmStatus ); /* send humidity alarm status */ /* to the serial port */ } /* getHumidityAlarmCmd() */ /****************************************************************************/ /* Function : getHumidityThreshCmd */ /* Purpose : Send humidity alarm threshold to serial port */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void getHumidityThreshCmd( Void ) { /* send humidity alarm thresh to serial port */ replyToCommand(3, CMD_OK, (Word) humidityAlarm.warnThresh, (Word) humidityAlarm.alarmThresh ); } /* getHumidityThreshCmd() */ /****************************************************************************/ /* Function : setHumidityThreshCmd */ /* Purpose : Process command to set humidity alarm threshold */ /* Inputs : Serial command buffer */ /* Outputs : None */ /****************************************************************************/ MLocal Void setHumidityThreshCmd(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read threshold from command buf */ wordsFromBuf( commandBuf, 3, &command, (Word *) &humidityAlarm.warnThresh, (Word *) &humidityAlarm.alarmThresh); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } /* setHumidityThreshCmd() */ /****************************************************************************/ /* Function : microGetWaterAlarmCmd */ /* Purpose : Process command to read water alarm status */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void microGetWaterAlarmCmd( Void ) { replyToCommand(2, CMD_OK, (Word) waterAlarmDetected); } /* microGetWaterAlarmCmd() */ /****************************************************************************/ /* Function : processApplicCommands */ /* Purpose : Decode application specific serial port commands */ /* Inputs : Command buffer and command buffer length */ /* Outputs : Return TRUE if command recognized and processed, else FALSE*/ /****************************************************************************/ MLocal Int16 processApplicCommands( Char *commandBuf, Int16 commandLen ) { /* try to recognize the command and call */ /* the appropriate handler */ switch (wordFromBuf(commandBuf, 0)) { case CAMERA_APP | GET_TEMPERATURE: microGetTemperature(); /* Get Temperature Sensor Value */ break; case CAMERA_APP | GET_TEMP_ALARM_STATUS: microGetTempAlarm(); /* Get Temperature Alarm Status */ break; case CAMERA_APP | SET_TEMP_ALARM_THRESH: microSetTempThresh(commandBuf); break; /* Set Temperature Alarm Threshold */ case CAMERA_APP | GET_TEMP_ALARM_THRESH: microGetTempThresh(); /* Get Temperature Alarm Threshold */ break; case CAMERA_APP | GET_HUMIDITY: getHumidityCmd(); /* Read Can Humidity Sensor Value */ break; case CAMERA_APP | GET_HUMIDITY_ALARM: getHumidityAlarmCmd(); /* Read state of humidity alarm */ break; case CAMERA_APP | GET_HUMIDITY_THRESH: getHumidityThreshCmd(); /* Get humidity alarm threshold */ break; case CAMERA_APP | SET_HUMIDITY_THRESH: setHumidityThreshCmd(commandBuf); break; /* Set humidity alarm threshold */ case CAMERA_APP | GET_WATER_ALARM: microGetWaterAlarmCmd();/* Send water alarm status */ break; /***************** Lens Focus Commands ******************************/ case CAMERA_APP | SET_FOCUS_SET_POINT: setFocusSetPoint(commandBuf); break; case CAMERA_APP | SET_FOCUS_VELOCITY_GAIN: setFocusVelocityGain(commandBuf); break; case CAMERA_APP | SET_FOCUS_CONTROL_MODE: setFocusControlMode(commandBuf); break; case CAMERA_APP | GET_FOCUS_SET_POINT: getFocusSetPoint(); break; case CAMERA_APP | GET_FOCUS_POSITION: getFocusPosition(); break; case CAMERA_APP | GET_FOCUS_VELOCITY_GAIN: getFocusVelocityGain(); break; case CAMERA_APP | GET_FOCUS_CONTROL_MODE: getFocusControlMode(); break; /***************** Lens Zoom Commands ******************************/ case CAMERA_APP | SET_ZOOM_SET_POINT: setZoomSetPoint(commandBuf); break; case CAMERA_APP | SET_ZOOM_VELOCITY_GAIN: setZoomVelocityGain(commandBuf); break; case CAMERA_APP | SET_ZOOM_CONTROL_MODE: setZoomControlMode(commandBuf); break; case CAMERA_APP | SET_ZOOM_RATE: setZoomRate(commandBuf); break; case CAMERA_APP | GET_ZOOM_SET_POINT: getZoomSetPoint(); break; case CAMERA_APP | GET_ZOOM_POSITION: getZoomPosition(); break; case CAMERA_APP | GET_ZOOM_VELOCITY_GAIN: getZoomVelocityGain(); break; case CAMERA_APP | GET_ZOOM_CONTROL_MODE: getZoomControlMode(); break; case CAMERA_APP | GET_ZOOM_RATE: getZoomRate(); break; case CAMERA_APP | GET_PWM_RATE: getPWMRate(); break; case CAMERA_APP | SET_PWM_RATE: setPWMRate(commandBuf); break; /************************* Camera Control Commands ******************/ case CAMERA_APP | GET_COLOR_BARS_MODE: getColorBarsModeCmd(); break; case CAMERA_APP | SET_COLOR_BARS_MODE: setColorBarsModeCmd(commandBuf); break; case CAMERA_APP | GET_MENU_MODE: getMenuModeCmd(); break; case CAMERA_APP | SET_MENU_MODE: setMenuModeCmd(commandBuf); break; case CAMERA_APP | GET_PAGE_STATE: getPageStateCmd(); break; case CAMERA_APP | SET_PAGE_STATE: setPageStateCmd(commandBuf); break; case CAMERA_APP | GET_ITEM_STATE: getItemStateCmd(); break; case CAMERA_APP | SET_ITEM_STATE: setItemStateCmd(commandBuf); break; case CAMERA_APP | GET_UP_STATE: getUpStateCmd(); break; case CAMERA_APP | SET_UP_STATE: setUpStateCmd(commandBuf); break; case CAMERA_APP | GET_DOWN_STATE: getDownStateCmd(); break; case CAMERA_APP | SET_DOWN_STATE: setDownStateCmd(commandBuf); break; case CAMERA_APP | GET_IRIS_MODE: getIrisModeCmd(); break; case CAMERA_APP | SET_IRIS_MODE: setIrisModeCmd(commandBuf); break; case CAMERA_APP | GET_IRIS_VALUE: getIrisValueCmd(); break; case CAMERA_APP | SET_IRIS_VALUE: setIrisValueCmd(commandBuf); break; case CAMERA_APP | GET_GAIN_MODE: getGainModeCmd(); break; case CAMERA_APP | SET_GAIN_MODE: setGainModeCmd(commandBuf); break; case CAMERA_APP | GET_GAIN_VALUE: getGainValueCmd(); break; case CAMERA_APP | SET_GAIN_VALUE: setGainValueCmd(commandBuf); break; case CAMERA_APP | GET_DETAIL_MODE: getDetailModeCmd(); break; case CAMERA_APP | SET_DETAIL_MODE: setDetailModeCmd(commandBuf); break; case CAMERA_APP | GET_SHUTTER_SPEED: getShutterSpeedCmd(); break; case CAMERA_APP | SET_SHUTTER_SPEED: setShutterSpeedCmd(commandBuf); break; case CAMERA_APP | GET_SCENE_FILE: getSceneFileCmd(); break; case CAMERA_APP | SET_SCENE_FILE: setSceneFileCmd(commandBuf); break; case CAMERA_APP | GET_AWC_MODE: getWhiteBalModeCmd(); break; case CAMERA_APP | SET_AWC_MODE: setWhiteBalModeCmd(commandBuf); break; case CAMERA_APP | GET_ATW_MODE: getAutoWhiteBalModeCmd(); break; case CAMERA_APP | SET_ATW_MODE: setAutoWhiteBalModeCmd(commandBuf); break; case CAMERA_APP | GET_TOTAL_PEDSTAL: getTotalPedstalCmd(); break; case CAMERA_APP | SET_TOTAL_PEDSTAL: setTotalPedstalCmd(commandBuf); break; case CAMERA_APP | GET_RED_PEDSTAL: getRedPedstalCmd(); break; case CAMERA_APP | SET_RED_PEDSTAL: setRedPedstalCmd(commandBuf); break; case CAMERA_APP | GET_BLUE_PEDSTAL: getBluePedstalCmd(); break; case CAMERA_APP | SET_BLUE_PEDSTAL: setBluePedstalCmd(commandBuf); break; case CAMERA_APP | GET_RED_GAIN: getRedGainCmd(); break; case CAMERA_APP | SET_RED_GAIN: setRedGainCmd(commandBuf); break; case CAMERA_APP | GET_BLUE_GAIN: getBlueGainCmd(); break; case CAMERA_APP | SET_BLUE_GAIN: setBlueGainCmd(commandBuf); break; default: return (FALSE); /* Command not recognized return FALSE */ } /* switch */ return (TRUE); /* Command recognized & processed */ } /* processApplicCommands() */ /****************************************************************************/ /* Function : executeCommand */ /* Purpose : Check commands received from serial port and call handler */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void executeCommand( Void ) { Char *commandBuf; /* Pointer to the command packet data. */ Int16 commandLen; /* Length of the command packet data */ /* read a command from serial port */ while (readRecvdPacket(&commandBuf, &commandLen) == OK) { if ( wordFromBuf(commandBuf, 0) == (MICRO_APP | CMD_ENABLE) ) { microEnabled = TRUE; replyToCommand(1, CMD_OK ); } /* if */ else if (microEnabled) { /* try to recognize the command and call*/ /* the appropriate handler */ if ( processApplicCommands( commandBuf, commandLen ) == FALSE ) processMicroCommands ( commandBuf, commandLen ); } /* if */ freeRecvdPacket(commandBuf);/* Free up the memory allocated for the */ /* packet by readSerialPacket() */ } /* while */ } /* executeCommand() */ /****************************************************************************/ /* Function : checkHumidityAlarm */ /* Purpose : Monitor humidity sensor and check alarm thresholds */ /* Inputs : None */ /* Outputs : None, but sends SRQ packet to serial port */ /****************************************************************************/ MLocal Void checkHumidityAlarm( Void ) { Byte ios0_copy; /* Read Humidity Sensor */ humidityAlarm.value = readHumidity(); /* Check if humidity has exceeded warn */ /* or alarm thresholds and send alarm */ /* SRQ's if necessary */ if (alarmThresholdCheck(humidityAlarm.value, humidityAlarm.warnThresh, humidityAlarm.alarmThresh, HUMIDITY_HYSTERESIS, &humidityAlarm.alarmStatus )==TRUE) if(humidityAlarm.alarmStatus == MICRO_ALARM) { writeServiceRequest(2, HUMIDITY_ALARM_SRQ, humidityAlarm.alarmStatus); disable(); ios0_copy = ios0; ios0_copy |= IOS0_HSO0_STATE; wsr = HWINDOW15; ios0 = ios0_copy; wsr = HWINDOW0; enable(); } else { writeServiceRequest(2, HUMIDITY_ALARM_SRQ, humidityAlarm.alarmStatus); disable(); ios0_copy = ios0; ios0_copy &= ~IOS0_HSO0_STATE; wsr = HWINDOW15; ios0 = ios0_copy; wsr = HWINDOW0; enable(); } } /* checkHumidityAlarm() */ /****************************************************************************/ /* Function : checkTempAlarm */ /* Purpose : Monitor temperature sensor and check alarm thresholds */ /* Inputs : None */ /* Outputs : None, but sends SRQ packet to serial port */ /****************************************************************************/ MLocal Void checkTempAlarm( Void ) { Byte ios0_copy; /* Read & Filter Temperature Sensor */ if (IIRFilter(readTemperature(TEMPERATURE_SENSOR_CHAN), &tempAlarm.value, &tempFilter)==OK) { /* Check if temperature has exceeded */ /* warn or alarm thresholds and send */ /* alarm SRQ's if necessary */ if (alarmThresholdCheck( tempAlarm.value, tempAlarm.warnThresh, tempAlarm.alarmThresh, TEMPERATURE_HYSTERESIS, &tempAlarm.alarmStatus )==TRUE) if(tempAlarm.alarmStatus == MICRO_ALARM) { writeServiceRequest(2, TEMP_ALARM_ON, tempAlarm.alarmStatus); disable(); ios0_copy = ios0; ios0_copy |= IOS0_HSO0_STATE; wsr = HWINDOW15; ios0 = ios0_copy; wsr = HWINDOW0; enable(); } else { writeServiceRequest(2, TEMP_ALARM_ON, tempAlarm.alarmStatus); disable(); ios0_copy = ios0; ios0_copy &= ~IOS0_HSO0_STATE; wsr = HWINDOW15; ios0 = ios0_copy; wsr = HWINDOW0; enable(); } }/*endif*/ } /* checkTempAlarm() */ /****************************************************************************/ /* Function : checkHdWaterAlarms */ /* Purpose : Checks water alarm status & send SRQ Packet sets HSO.0 */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ Void checkHdWaterAlarm( Void ) { Boolean oldAlarmStatus; /* Copy of previous alarm status */ Byte ios0_copy; oldAlarmStatus = waterAlarmDetected;/* Save old state of water alarm */ if ((waterAlarmDetected = readWaterAlarms()) != oldAlarmStatus) if (waterAlarmDetected == TRUE) { writeServiceRequest(1, WATER_ALARM_ON_SRQ); disable(); ios0_copy = ios0; ios0_copy |= IOS0_HSO0_STATE; wsr = HWINDOW15; ios0 = ios0_copy; wsr = HWINDOW0; enable(); } else { writeServiceRequest(1, WATER_ALARM_OFF_SRQ); disable(); ios0_copy = ios0; ios0_copy &= ~IOS0_HSO0_STATE; wsr = HWINDOW15; ios0 = ios0_copy; wsr = HWINDOW0; enable(); } } /* checkHdWaterAlarm() */ /************************************************ * * * Set/Get Focus Parameters * * * ************************************************/ MLocal Void setFocusSetPoint(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read Set Point from command buf */ wordsFromBuf( commandBuf, 2, &command, (Word *) &setPoint[FOCUS_SERVO]); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } /* setFocusSetPoint */ MLocal Void getFocusSetPoint( Void ) { replyToCommand(2, CMD_OK, setPoint[FOCUS_SERVO]); }/* getFocusSetPoint */ MLocal Void getFocusPosition( Void ) { replyToCommand(2, CMD_OK, position[FOCUS_SERVO]); }/* getFocusPosition */ MLocal Void setFocusVelocityGain(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read Velocity Gain from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &velocityGain[FOCUS_SERVO]); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } /* setFocusVelocityGain */ MLocal Void getFocusVelocityGain( Void ) { replyToCommand(2, CMD_OK, velocityGain[FOCUS_SERVO]); }/* getFocusVelocityGain */ MLocal Void setFocusControlMode(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read Feedback from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &controlMode[ZOOM_SERVO]); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } /* setFocusControlMode */ MLocal Void getFocusControlMode( Void ) { replyToCommand(2, CMD_OK, controlMode[ZOOM_SERVO]); }/* getFocusControlMode */ /************************************************ * * * Set / Get Zoom Parameters * * * ************************************************/ MLocal Void setZoomSetPoint(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read Set Point from command buf */ wordsFromBuf( commandBuf, 2, &command, (Word *) &zoomInput); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } /* setZoomSetPoint */ MLocal Void getZoomSetPoint( Void ) { replyToCommand(2, CMD_OK, setPoint[ZOOM_SERVO]); }/* getZoomSetPoint */ MLocal Void getZoomPosition( Void ) { replyToCommand(2, CMD_OK, position[ZOOM_SERVO]); }/* getZoomSetPoint */ MLocal Void setZoomVelocityGain(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read Velocity FeedBack from command buf */ wordsFromBuf( commandBuf, 2, &command, (Word *) &velocityGain[ZOOM_SERVO]); readTachOffset(); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setZoomVelocityGain */ MLocal Void getZoomVelocityGain( Void ) { replyToCommand(2, CMD_OK, velocityGain[ZOOM_SERVO]); }/* getZoomVelocityGain */ MLocal Void setZoomControlMode(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read Control Mode from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &controlMode[ZOOM_SERVO]); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } /* setZoomControlMode */ MLocal Void getZoomControlMode( Void ) { replyToCommand(2, CMD_OK, controlMode[ZOOM_SERVO]); }/* getZoomControlMode */ MLocal Void setZoomRate(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read Zoom Rate from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &zoomRate); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } /* setZoomRate */ MLocal Void getZoomRate( Void ) { replyToCommand(2, CMD_OK, zoomRate); }/* getZoomRate */ /**************************************** * * * Update Timing Parameters * * * ****************************************/ MLocal Void getPWMRate( Void ) { replyToCommand(2, CMD_OK, totalPwmRate); }/* getPWMRate */ MLocal Void setPWMRate(Byte *commandBuf) { Word command; /* Command from serial port */ /* Read PWM Rate from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &totalPwmRate); pwmRate = totalPwmRate - 50; replyToCommand(1, CMD_OK); /* Send command OK to serial port */ } MLocal Void checkCameraLink( Void ) { Byte buffer; buffer = DCTM; if (cameraLink == CAMERA_LINK_DOWN) camUartSerialChanWrite(CAM_SERIAL_CHAN, &buffer, 1); } /* checkCameraLink */ MLocal Void initDataResp( Void ) { disable(); data_resp[DATA1][BYTE0] = DATA1_0; data_resp[DATA1][BYTE1] = DATA1_1; data_resp[DATA1][BYTE2] = DATA1_2; data_resp[DATA1][CHECKSUM] = DATA1_CS; data_resp[DATA2][BYTE0] = DATA2_0; data_resp[DATA2][BYTE1] = DATA2_1; data_resp[DATA2][BYTE2] = DATA2_2; data_resp[DATA2][CHECKSUM] = DATA2_CS; data_resp[DATA3][BYTE0] = DATA3_0; data_resp[DATA3][BYTE1] = DATA3_1; data_resp[DATA3][BYTE2] = DATA3_2; data_resp[DATA3][CHECKSUM] = DATA3_CS; data_resp[DATA4][BYTE0] = DATA4_0; data_resp[DATA4][BYTE1] = DATA4_1; data_resp[DATA4][BYTE2] = DATA4_2; data_resp[DATA4][CHECKSUM] = DATA4_CS; data_resp[DATA5][BYTE0] = DATA5_0; data_resp[DATA5][BYTE1] = DATA5_1; data_resp[DATA5][BYTE2] = DATA5_2; data_resp[DATA5][CHECKSUM] = DATA5_CS; data_resp[DATA6][BYTE0] = DATA6_0; data_resp[DATA6][BYTE1] = DATA6_1; data_resp[DATA6][BYTE2] = DATA6_2; data_resp[DATA6][CHECKSUM] = DATA6_CS; data_resp[DATA7][BYTE0] = DATA7_0; data_resp[DATA7][BYTE1] = DATA7_1; data_resp[DATA7][BYTE2] = DATA7_2; data_resp[DATA7][CHECKSUM] = DATA7_CS; data_resp[DATA8][BYTE0] = DATA8_0; data_resp[DATA8][BYTE1] = DATA8_1; data_resp[DATA8][BYTE2] = DATA8_2; data_resp[DATA8][CHECKSUM] = DATA8_CS; data_resp[CCC][BYTE0] = CCC_0; data_resp[CCC][BYTE1] = CCC_1; data_resp[CCC][BYTE2] = CCC_2; data_resp[CCC][CHECKSUM] = CCC_CS; colorBarsMode = BARS_OFF; menuMode = MENU_OFF; pageState = PAGE_OFF; itemState = ITEM_OFF; upState = UP_OFF; downState = DOWN_OFF; irisMode = AUTO_IRIS_ON; irisValue = 0; gainMode = AUTO_GAIN_OFF; gainValue = GAIN_0DB; detailMode = DETAIL_LOW; shutterSpeed = SHUTTER_OFF; sceneFile = SCENE_USER; whiteBalMode = HOLD; autoWhiteBalMode = ATW; totalPedstal = 0x88; redPedstal = 0x88; bluePedstal = 0x88; redGain = 0x88; blueGain = 0x88; enable(); }/* initDataResp */ /****************************************************************************/ MLocal Void setColorBarsModeCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Boolean state; /* Read Color Bars Mode from command buf*/ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setColorBars(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setColorBarsModeCmd */ MLocal Void getColorBarsModeCmd( Void ) { replyToCommand(2, CMD_OK, colorBarsMode ); } /* getColorBarsModeCmd */ MLocal Void setColorBars(Boolean state) { disable(); if (state == BARS_ON) data_resp[DATA1][BYTE1] |= BAR_RSP; else data_resp[DATA1][BYTE1] &= ~BAR_RSP; data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); colorBarsMode = state; enable(); } MLocal Void setMenuModeCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Boolean state; /* Read Menu Mode from command buf*/ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setMenuMode(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setMenuModeCmd */ MLocal Void getMenuModeCmd( Void ) { replyToCommand(2, CMD_OK, menuMode ); }/* getMenuModeCmd */ MLocal Void setMenuMode(Boolean state) { disable(); if (state == MENU_ON) data_resp[DATA8][BYTE2] |= MENU_RSP; else data_resp[DATA8][BYTE2] &= ~MENU_RSP; data_resp[DATA8][CHECKSUM] = -(data_resp[DATA8][BYTE0] + data_resp[DATA8][BYTE1] + data_resp[DATA8][BYTE2]); menuMode = state; enable(); } MLocal Void setPageStateCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Boolean state; /* Read Page State from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setPageState(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setPageStateCmd */ MLocal Void getPageStateCmd( Void ) { replyToCommand(2, CMD_OK, pageState ); }/* getPageStateCmd */ MLocal Void setPageState(Boolean state) { disable(); if (state == PAGE_ON) data_resp[DATA1][BYTE0] |= PAGE_RSP; else data_resp[DATA1][BYTE0] &= ~PAGE_RSP; data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); pageState = state; enable(); }/* setPageState */ MLocal Void setItemStateCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Boolean state; /* Read Item State from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setItemState(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setItemStateCmd */ MLocal Void getItemStateCmd( Void ) { replyToCommand(2, CMD_OK, itemState ); }/* getItemStateCmd */ MLocal Void setItemState(Boolean state) { disable(); if (state == ITEM_ON) data_resp[DATA1][BYTE0] |= ITEM_RSP; else data_resp[DATA1][BYTE0] &= ~ITEM_RSP; data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); itemState = state; enable(); }/* setItemState */ MLocal Void setUpStateCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Boolean state; /* Read Up State from command buf*/ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setUpState(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setUpStateCmd */ MLocal Void getUpStateCmd( Void ) { replyToCommand(2, CMD_OK, upState ); }/* getUpStateCmd */ MLocal Void setUpState(Boolean state) { disable(); if (state == UP_ON) data_resp[DATA1][BYTE0] |= UP_RSP; else data_resp[DATA1][BYTE0] &= ~UP_RSP; data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); upState = state; enable(); }/* setUpState */ MLocal Void setDownStateCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Boolean state; /* Read Down State from command buf*/ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setDownState(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setDownStateCmd */ MLocal Void getDownStateCmd( Void ) { replyToCommand(2, CMD_OK, downState ); }/* getDownStateCmd */ MLocal Void setDownState(Boolean state) { disable(); if (state == UP_ON) data_resp[DATA1][BYTE0] |= DOWN_RSP; else data_resp[DATA1][BYTE0] &= ~DOWN_RSP; data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); downState = state; enable(); }/* setDownState */ MLocal Void setIrisModeCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Boolean state; /* Read Iris Mode from command buf*/ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setIrisMode(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setIrisModeCmd */ MLocal Void getIrisModeCmd( Void ) { replyToCommand(2, CMD_OK, irisMode ); }/* getIrisModeCmd */ MLocal Void setIrisMode(Boolean state) { disable(); if (state == AUTO_IRIS_ON) data_resp[DATA2][BYTE0] |= AUTO_IRIS_RSP; else data_resp[DATA2][BYTE0] &= ~AUTO_IRIS_RSP; data_resp[DATA2][CHECKSUM] = -(data_resp[DATA2][BYTE0] + data_resp[DATA2][BYTE1] + data_resp[DATA2][BYTE2]); irisMode = state; enable(); } MLocal Void setIrisValueCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 value; /* Read Iris Mode from command buf*/ wordsFromBuf(commandBuf, 2, &command, (Word *) &value); setIrisValue(value); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setIrisValueCmd */ MLocal Void getIrisValueCmd( Void ) { replyToCommand(2, CMD_OK, irisValue ); }/* getIrisValueCmd */ MLocal Void setIrisValue(Int16 value) { disable(); if (irisMode == AUTO_IRIS_OFF) { data_resp[DATA7][BYTE2] = (Byte)(value & 0x00FF); data_resp[DATA7][BYTE1] = (Byte)((value & 0x0300)>>8); } else if (irisMode == AUTO_IRIS_ON) data_resp[DATA7][BYTE1] = data_resp[DATA7][BYTE2] = 0; data_resp[DATA7][CHECKSUM] = -(data_resp[DATA7][BYTE0] + data_resp[DATA7][BYTE1] + data_resp[DATA7][BYTE2]); irisValue = value; enable(); }/*setIrisValue*/ MLocal Void setGainModeCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 state; /* Read Gain Mode from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setGainMode(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setGainModeCmd */ MLocal Void getGainModeCmd( Void ) { replyToCommand(2, CMD_OK, gainMode ); }/* getGainModeCmd */ MLocal Void setGainMode(Int16 state) { disable(); if (state == AUTO_GAIN_LOW) { data_resp[DATA2][BYTE0] &= ~AGC_LOW_RSP; } else if (state == AUTO_GAIN_OFF) { data_resp[DATA2][BYTE0] &= ~AGC_LOW_RSP; data_resp[DATA2][BYTE0] |= AGC_OFF_RSP; } else if (state == AUTO_GAIN_HIGH) { data_resp[DATA2][BYTE0] &= ~AGC_LOW_RSP; data_resp[DATA2][BYTE0] |= AGC_HIGH_RSP; } data_resp[DATA2][CHECKSUM] = -(data_resp[DATA2][BYTE0] + data_resp[DATA2][BYTE1] + data_resp[DATA2][BYTE2]); gainMode = state; enable(); } MLocal Void setGainValueCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 value; /* Read Gain Mode from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &value); setGainValue(value); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setGainValueCmd */ MLocal Void getGainValueCmd( Void ) { replyToCommand(2, CMD_OK, gainValue ); }/* getGainValueCmd */ MLocal Void setGainValue(Int16 value) { disable(); if (gainMode == AUTO_GAIN_OFF) { if (value == GAIN_0DB) { data_resp[DATA2][BYTE0] &= ~GAIN_0dB_RSP; } else if (value == GAIN_9DB) { data_resp[DATA2][BYTE0] &= ~GAIN_0dB_RSP; data_resp[DATA2][BYTE0] |= GAIN_9dB_RSP; } else if (value == GAIN_18DB) { data_resp[DATA2][BYTE0] &= ~GAIN_0dB_RSP; data_resp[DATA2][BYTE0] |= GAIN_18dB_RSP; } data_resp[DATA2][CHECKSUM] = -(data_resp[DATA2][BYTE0] + data_resp[DATA2][BYTE1] + data_resp[DATA2][BYTE2]); } gainValue = value; enable(); }/* setGainValue */ MLocal Void setDetailModeCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 state; /* Read Detail Mode from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setDetailMode(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setDetailModeCmd */ MLocal Void getDetailModeCmd( Void ) { replyToCommand(2, CMD_OK, detailMode ); }/* getDetailModeCmd */ MLocal Void setDetailMode(Int16 state) { disable(); if (state == DETAIL_OFF) { data_resp[DATA1][BYTE1] &= ~DETAIL_OFF_RSP; detailMode = DETAIL_OFF; } else if (state == DETAIL_HIGH) { data_resp[DATA1][BYTE1] &= ~DETAIL_OFF_RSP; data_resp[DATA1][BYTE1] |= DETAIL_HIGH_RSP; detailMode = DETAIL_HIGH; } else if (state == DETAIL_LOW) { data_resp[DATA1][BYTE1] &= ~DETAIL_OFF_RSP; data_resp[DATA1][BYTE1] |= DETAIL_LOW_RSP; detailMode = DETAIL_LOW; } data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); enable(); } MLocal Void setShutterSpeedCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 state; /* Read Shutter Speed from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setShutterSpeed(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setShutterSpeedCmd */ MLocal Void getShutterSpeedCmd( Void ) { replyToCommand(2, CMD_OK, shutterSpeed ); }/* getShutterSpeedCmd */ MLocal Void setShutterSpeed(Int16 state) { disable(); if (state == SHUTTER_OFF) data_resp[DATA2][BYTE1] &= ~SHUTTER_OFF_RSP; else if (state == SHUTTER_100) { data_resp[DATA2][BYTE1] &= ~SHUTTER_OFF_RSP; data_resp[DATA2][BYTE1] |= SHUTTER_100_RSP; } else if (state == SHUTTER_500) { data_resp[DATA2][BYTE1] &= ~SHUTTER_OFF_RSP; data_resp[DATA2][BYTE1] |= SHUTTER_500_RSP; } else if (state == SHUTTER_1000) { data_resp[DATA2][BYTE1] &= ~SHUTTER_OFF_RSP; data_resp[DATA2][BYTE1] |= SHUTTER_1000_RSP; } else if (state == SHUTTER_SYNCHRO) { data_resp[DATA2][BYTE1] &= ~SHUTTER_OFF_RSP; data_resp[DATA2][BYTE1] |= SHUTTER_SYNCHRO_RSP; } else if (state == SHUTTER_ELC) { data_resp[DATA2][BYTE1] &= ~SHUTTER_OFF_RSP; data_resp[DATA2][BYTE1] |= SHUTTER_ELC_RSP; } data_resp[DATA2][CHECKSUM] = -(data_resp[DATA2][BYTE0] + data_resp[DATA2][BYTE1] + data_resp[DATA2][BYTE2]); shutterSpeed = state; enable(); } MLocal Void setSceneFileCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 state; /* Read Scene File from command buf */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setSceneFile(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setSceneFileCmd */ MLocal Void getSceneFileCmd( Void ) { replyToCommand(2, CMD_OK, sceneFile ); }/* getSceneFileCmd */ MLocal Void setSceneFile(Int16 state) { disable(); if (state == SCENE_USER) data_resp[DATA2][BYTE1] &= ~SCENE_FILE_USER_RSP; else if (state == SCENE_1) { data_resp[DATA2][BYTE1] &= ~SCENE_FILE_USER_RSP; data_resp[DATA2][BYTE1] |= SCENE_FILE_1_RSP; } else if (state == SCENE_2) { data_resp[DATA2][BYTE1] &= ~SCENE_FILE_USER_RSP; data_resp[DATA2][BYTE1] |= SCENE_FILE_2_RSP; } else if (state == SCENE_3) { data_resp[DATA2][BYTE1] &= ~SCENE_FILE_USER_RSP; data_resp[DATA2][BYTE1] |= SCENE_FILE_3_RSP; } data_resp[DATA2][CHECKSUM] = -(data_resp[DATA2][BYTE0] + data_resp[DATA2][BYTE1] + data_resp[DATA2][BYTE2]); sceneFile = state; enable(); } MLocal Void setWhiteBalModeCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 state; /* Read White Balance Mode */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setWhiteBalMode(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setWhiteBalModeCmd */ MLocal Void getWhiteBalModeCmd( Void ) { replyToCommand(2, CMD_OK, whiteBalMode ); }/* getWhiteBalModeCmd */ MLocal Void setWhiteBalMode(Int16 state) { disable(); if (state == HOLD) data_resp[DATA1][BYTE0] &= ~AWC_NON_RSP; else if (state == AWC) { data_resp[DATA1][BYTE0] &= ~AWC_NON_RSP; data_resp[DATA1][BYTE0] |= AWC_RSP; } else if (state == ABC) { data_resp[DATA1][BYTE0] &= ~AWC_NON_RSP; data_resp[DATA1][BYTE0] |= ABC_RSP; } data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); whiteBalMode = state; enable(); } MLocal Void setAutoWhiteBalModeCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 state; /* Read Auto White Balance Mode */ wordsFromBuf(commandBuf, 2, &command, (Word *) &state); setAutoWhiteBalMode(state); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setAutoWhiteBalModeCmd */ MLocal Void getAutoWhiteBalModeCmd( Void ) { replyToCommand(2, CMD_OK, autoWhiteBalMode ); }/* getAutoWhiteBalModeCmd */ MLocal Void setAutoWhiteBalMode(Int16 state) { disable(); if (state == ATW) data_resp[DATA1][BYTE1] &= ~ATW_RSP; else if (state == ACH) { data_resp[DATA1][BYTE1] &= ~ATW_RSP; data_resp[DATA1][BYTE1] |= ACH_RSP; } else if (state == BCH) { data_resp[DATA1][BYTE1] &= ~ATW_RSP; data_resp[DATA1][BYTE1] |= BCH_RSP; } data_resp[DATA1][CHECKSUM] = -(data_resp[DATA1][BYTE0] + data_resp[DATA1][BYTE1] + data_resp[DATA1][BYTE2]); autoWhiteBalMode = state; enable(); }/* setAutoWhiteBalMode */ MLocal Void setTotalPedstalCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 value; /* Read Total Pedstal Value */ wordsFromBuf(commandBuf, 2, &command, (Word *) &value); setTotalPedstal(value); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setTotalPedstalCmd */ MLocal Void getTotalPedstalCmd( Void ) { replyToCommand(2, CMD_OK, totalPedstal ); }/* getTotalPedstalCmd */ MLocal Void setTotalPedstal(Int16 value) { disable(); data_resp[DATA2][BYTE2] = (Byte) value; data_resp[DATA2][CHECKSUM] = -(data_resp[DATA2][BYTE0] + data_resp[DATA2][BYTE1] + data_resp[DATA2][BYTE2]); totalPedstal = value; enable(); }/* setTotalPedstal */ MLocal Void setRedPedstalCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 value; /* Read Red Pedstal Value */ wordsFromBuf(commandBuf, 2, &command, (Word *) &value); setRedPedstal(value); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setRedPedstalCmd */ MLocal Void getRedPedstalCmd( Void ) { replyToCommand(2, CMD_OK, redPedstal ); }/* getRedPedstalCmd */ MLocal Void setRedPedstal(Int16 value) { disable(); data_resp[DATA3][BYTE1] = (Byte) value; data_resp[DATA3][CHECKSUM] = -(data_resp[DATA3][BYTE0] + data_resp[DATA3][BYTE1] + data_resp[DATA3][BYTE2]); redPedstal = value; enable(); }/* setRedPedstal */ MLocal Void setBluePedstalCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 value; /* Read Blue Pedstal Value */ wordsFromBuf(commandBuf, 2, &command, (Word *) &value); setBluePedstal(value); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setBluePedstalCmd */ MLocal Void getBluePedstalCmd( Void ) { replyToCommand(2, CMD_OK, bluePedstal ); }/* getBluePedstalCmd */ MLocal Void setBluePedstal(Int16 value) { disable(); data_resp[DATA3][BYTE2] = (Byte) value; data_resp[DATA3][CHECKSUM] = -(data_resp[DATA3][BYTE0] + data_resp[DATA3][BYTE1] + data_resp[DATA3][BYTE2]); bluePedstal = value; enable(); }/* setBluePedstal */ MLocal Void setRedGainCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 value; /* Read Red Gain Value */ wordsFromBuf(commandBuf, 2, &command, (Word *) &value); setRedGain(value); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setRedGainCmd */ MLocal Void getRedGainCmd( Void ) { replyToCommand(2, CMD_OK, redGain ); }/* getRedGainCmd */ MLocal Void setRedGain(Int16 value) { disable(); data_resp[DATA4][BYTE0] = (Byte) value; data_resp[DATA4][CHECKSUM] = -(data_resp[DATA4][BYTE0] + data_resp[DATA4][BYTE1] + data_resp[DATA4][BYTE2]); redGain = value; enable(); }/* setRedGain */ MLocal Void setBlueGainCmd(Byte *commandBuf) { Word command; /* Command from serial port */ Int16 value; /* Read Blue Gain Value */ wordsFromBuf(commandBuf, 2, &command, (Word *) &value); setBlueGain(value); replyToCommand(1, CMD_OK); /* Send command OK to serial port */ }/* setBlueGainCmd */ MLocal Void getBlueGainCmd( Void ) { replyToCommand(2, CMD_OK, blueGain ); }/* getBlueGainCmd */ MLocal Void setBlueGain(Int16 value) { disable(); data_resp[DATA4][BYTE1] = (Byte) value; data_resp[DATA4][CHECKSUM] = -(data_resp[DATA4][BYTE0] + data_resp[DATA4][BYTE1] + data_resp[DATA4][BYTE2]); blueGain = value; enable(); }/* setBlueGain */ /****************************************************************************/ /* Function : servo */ /* Purpose : Called from main loop causes servo actions to execute */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void servo( Void ) { servoAction(ZOOM_SERVO); servoAction(FOCUS_SERVO); } /* Servo */ /****************************************************************************/ /* Function : servoAction */ /* Purpose : Processes Camera ZOOM and FOCUS servo actions */ /* Inputs : controller */ /* Outputs : None */ /****************************************************************************/ MLocal Void servoAction(Int16 controller) { measure(controller); /* Measure Tachometer and Position */ controlLaw(controller); /* Calculate servo command */ stallCheck(controller); /* Check for motor stall */ /* Output servo command on IOPORT1 */ if (controller == ZOOM_SERVO) zoomOutput(servoCommand[ZOOM_SERVO]); else focusOutput(servoCommand[FOCUS_SERVO]); /* Update last set point */ prevSetPoint[controller] = setPoint[controller]; } /* servoAction */ /****************************************************************************/ /* Function : zoomOutput */ /* Purpose : Processes Camera ZOOM command */ /* Inputs : zoomCommand */ /* Outputs : None */ /* */ /* The + direction for zoom yields HIGHER pot feedback values, and causes */ /* the lens to zoom IN. */ /****************************************************************************/ MLocal Void zoomOutput(Int16 zoomCommand ) { disable(); /* disable interrupts */ if (zoomCommand < 0) { /* if zoom command negative */ /* get absolute value */ zoomCommand = -zoomCommand; /* Set Port Image turn servo - direction*/ port1Image |= 0x01; /* P1.0 = 1, blk wire is pos */ port1Image &= ~ 0x04; /* P1.2 = 0, red wire is neg */ } else { /* if zoom command positive /* Set Port Image turn servo + direction*/ port1Image &= ~ 0x01; /* P1.0 = 0, blk wire is neg */ port1Image |= 0x04; /* P1.2 = 1, red wire is pos */ } /* end if */ /* Maximum Limit */ if (zoomCommand > pwmRate - 50) zoomCommand = pwmRate - 50; zoomMotorOffTime = pwmRate - zoomCommand; enable(); /* enable interrupts */ } /* zoomOutput */ /****************************************************************************/ /* Function : focusOutput */ /* Purpose : Processes Camera FOCUS command */ /* Inputs : focusCommand */ /* Outputs : None */ /* */ /* The + direction for focus yields HIGHER pot feedback values, and causes */ /* the lens to focus FAR. */ /****************************************************************************/ MLocal Void focusOutput(Int16 focusCommand) { disable(); /* disable interrupts */ if (focusCommand < 0) { /* if focus command negative */ /* absolute value */ focusCommand = -focusCommand; /* Set Port Image turn servo - direction*/ port1Image &= ~ 0x02; /* P1.1 = 0, blk wire is neg */ port1Image |= 0x08; /* P1.3 = 1, red wire is pos */ } else { /* if focus command positive */ /* Set Port Image turn servo + direction*/ port1Image |= 0x02; /* P1.1 = 1, blk wire is pos */ port1Image &= ~ 0x08; /* P1.3 = 0, red wire is neg */ } /* end if */ /* Maximum Limit */ if (focusCommand > pwmRate - 50) focusCommand = pwmRate - 50; focusMotorOffTime = pwmRate - focusCommand; enable(); /* enable interrupts */ }/* focusOutput */ /****************************************************************************/ /* Function : controlLaw */ /* Purpose : Servo PID control Law */ /* Inputs : controller */ /* Outputs : None */ /****************************************************************************/ MLocal Void controlLaw(Int16 controller) { Int16 servoError, deltaPosition; /* Calculate servo error */ /* Note: error is usually defined as */ /* actual - desired, but here we use */ /* the opposite so that when actual is */ /* GREATER than desired, we'll get a */ /* NEGATIVE servo command. */ if(controller == ZOOM_SERVO && controlMode[ZOOM_SERVO] == RATE_MODE) { servoError = (10 * zoomInput) - tachometer; } else { setPoint[ZOOM_SERVO] = zoomInput; servoError = setPoint[controller] - position[controller]; } /* end if */ /* Dead banding */ if((servoError <= 5) && (servoError >= -5)) servoError = 0; deltaPosition = position[controller] - prevError[controller]; prevError[controller] = position[controller]; #if 0 /* substitute simple control law for DEBUG */ if(servoError == 0) { servoCommand[controller] = 0; } else if (servoError > 0) { servoCommand[controller] = 450; } else { servoCommand[controller] = -450; } /* end if */ #endif /* Calculate proportional part */ servoCommand[controller] = scaleMultiply(servoError, propK[controller][controlMode[controller]]); /* Check if we are in proportional band*/ /* If not give maximum servo drive */ if(servoCommand[controller] > MAX_SERVO_COMMAND) { servoCommand[controller] = MAX_SERVO_COMMAND; } else if(servoCommand[controller] < -MAX_SERVO_COMMAND) { servoCommand[controller] = -MAX_SERVO_COMMAND; } else { /* We are in the proportional band */ /* so add in other control law parts */ integError[controller] += servoError; /* Check for error overflow */ limit(integError[controller], MAX_ERR); /* Add in integral part */ servoCommand[controller] += scaleMultiply(integError[controller], integK[controller][controlMode[controller]]); /* Add in differential Part */ servoCommand[controller] -= scaleMultiply(deltaPosition, diffK[controller][controlMode[controller]]); } /* end if */ /* Check for servoCommand overflow */ limit(servoCommand[controller], MAX_SERVO_COMMAND); } /* controlLaw */ /****************************************************************************/ /* Function : stallCheck */ /* Purpose : Check for servo stall */ /* Inputs : controller */ /* Outputs : None */ /****************************************************************************/ MLocal Void stallCheck(Int16 controller) { MLocal Int16 stallCount[NUMBER_OF_SERVOS]; Int16 speed; Int16 setPointDelta; speed = position[controller] - prevPosition[controller]; prevPosition[controller] = position[controller]; if (speed < 0) /* Get the absolute value of speed */ speed = -speed; setPointDelta = prevSetPoint[controller] - setPoint[controller]; if (setPointDelta < 0) /* get absolute value of setPointDelta */ setPointDelta = -setPointDelta; /* If no motion then add to counter */ if ((speed < SLOW_SPEED) && (setPointDelta < 1)) { /* Going slow and no new commands */ if (stallCount[controller] < STALL_COUNT_LIMIT) { stallCount[controller]++; } else { /* Stalled too long so kill servo */ servoCommand[controller] = 0; if(controller == ZOOM_SERVO) readTachOffset(); /* We're not moving so get tach offset */ } /* end if */ } else { /* Not stalled so clear counter */ stallCount[controller] = 0; } /* end if */ } /* stallCheck */ /****************************************************************************/ /* Function : measure */ /* Purpose : measures servo position and tachometer */ /* Inputs : controller */ /* Outputs : None */ /****************************************************************************/ MLocal Void measure(Int16 controller) { Int16 iters; Int32 sum = 0; if(controller == ZOOM_SERVO) { /* Read average position of zoom servo */ setPort2Bit(); for (iters = 0; iters < CLOSE_COUNT; iters++) sum += readAnalogChannel(ZOOM_CHANNEL) + 512; position[ZOOM_SERVO] = (Int16)(sum / CLOSE_COUNT); clrPort2Bit(); /* If zoom servo in rate mode read tach */ if(controlMode[ZOOM_SERVO] == RATE_MODE) { tachometer = readAnalogChannel(ZOOM_TACH_CHANNEL) + 512 - tachOffset; } /* end if */ } else { /* Read average position of focus servo */ for (iters = 0; iters < CLOSE_COUNT; iters++) sum += readAnalogChannel(FOCUS_CHANNEL) + 512; position[FOCUS_SERVO] = (Int16)(sum / CLOSE_COUNT); } /* end if */ } /* end measure */ /****************************************************************************/ /* Function : readTachOffset */ /* Purpose : reads zoom tachometer offset */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void readTachOffset() { Nat16 iters; /* Local status variables */ Int32 voltage = 0; for (iters = 0; iters < CLOSE_COUNT; iters++) voltage += readAnalogChannel(ZOOM_TACH_CHANNEL) + 512; voltage /= CLOSE_COUNT; /* Calculate average analog voltage */ tachOffset = (Int16)voltage; /* update tach offset */ }/* readTachOffset */ /****************************************************************************/ /* Function : initServos */ /* Purpose : initializes servo software */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal Void initServos() { /* Initialize New Control Law Constants*/ propK[FOCUS_SERVO][POSITION_MODE] = 400; propK[ZOOM_SERVO][POSITION_MODE] = 6 * 256; propK[ZOOM_SERVO][RATE_MODE] = 200; integK[FOCUS_SERVO][POSITION_MODE] = 84; integK[ZOOM_SERVO][POSITION_MODE] = 256; integK[ZOOM_SERVO][RATE_MODE] = 0; diffK[FOCUS_SERVO][POSITION_MODE] = 300; diffK[ZOOM_SERVO][POSITION_MODE] = 5 * 256; diffK[ZOOM_SERVO][RATE_MODE] = 0; integError[FOCUS_SERVO] = integError[ZOOM_SERVO] = 0; /* Initialize PWM values */ pwmRate = 450; totalPwmRate = 500; zoomMotorOffTime = 250; focusMotorOffTime = 250; /* Initialize Set Points */ zoomInput = 0; setPoint[ZOOM_SERVO] = prevSetPoint[ZOOM_SERVO] = 100; /* zoom out */ setPoint[FOCUS_SERVO] = prevSetPoint[FOCUS_SERVO] = 1000; /* focus far */ /* Select Initial Control Modes */ controlMode[ZOOM_SERVO] = POSITION_MODE; controlMode[FOCUS_SERVO] = POSITION_MODE; /* Initialize Feedback Gains */ velocityGain[ZOOM_SERVO] = 0;/*200;*/ velocityGain[FOCUS_SERVO] = 0; /* Initialize Tachometer Offset */ readTachOffset(); zoomRate = 450; start_pwm(); /*zeroServos();*/ /* Run servos to zero Positions */ /* Initialize Position Offsets */ initOffset(ZOOM_SERVO); initOffset(FOCUS_SERVO); /* Initialize servo errors */ prevError[ZOOM_SERVO] = prevError[FOCUS_SERVO] = 0; } /* initServos */ Void setPort2Bit( Void ) { setIoport2Bit(0x40); } /* setPort2Bit */ Void clrPort2Bit( Void ) { clrIoport2Bit(0x40); } /* clrPort2Bit */ MLocal Void initOffset(Int16 controller) { Int16 voltage = 0; Nat16 iters; /* Local status variables */ if (controller == ZOOM_SERVO) { /* do zoom servo */ for (iters = 0; iters < CLOSE_COUNT; iters++) voltage += readAnalogChannel(ZOOM_CHANNEL) + 512; voltage /= CLOSE_COUNT; /* Calculate average analog voltage */ } else { /* do focus servo */ for (iters = 0; iters < CLOSE_COUNT; iters++) voltage += readAnalogChannel(FOCUS_CHANNEL) + 512; voltage /= CLOSE_COUNT; /* Calculate average analog voltage */ } /* end if */ positionOffset[controller] = voltage; } /* initOffset */ MLocal Void zeroServos( Void ) { Int16 count; for ( count = 0; count < 2000; count++) servo(); }/* zeroServos */ MLocal Int16 scaleMultiply(Int16 a, Int16 b) { Int32 product; product =((((Int32) a) * b)>>8); limit(product, 32767); return((Int16) product); }/* scaleMultiply */