/****************************************************************************/ /* Copyright 1996 MBARI */ /****************************************************************************/ /* Summary : Belly Pack Microcontroller Interface Module for VxWorks */ /* Filename : bellyPack.c */ /* Author : Douglas Au */ /* Project : New ROV */ /* Version : Version 1.0 */ /* Created : 04/16/96 */ /* Modified : 08/13/96 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: bellyPack.c,v 1.1 96/04/16 11:30:21 audo Exp $ * $Log: bellyPack.c,v $ * Revision 1.1 95/04/16 11:30:21 11:30:21 audo (Douglas Au) * Initial revision * */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks standard IO file functions */ #include /* vxWorks semaphore functions */ #include /* vxWorks task library functions */ #include /* vxWorks linked list library */ #include /* vxWorks watch dog timer functions */ #include /* vxWorks time and date functions */ #include /* vxWorks Message Queue Library */ #include /* MBARI style guide type declarations */ #include /* Miscellaneous constants */ #include /* MBARI Trig Functions */ #include /* MBARI Rov Application Priorities */ #include /* MBARI time and date functions */ #include /* Data Manager declarations */ #include /* Data Manager error declarations */ #include "sio32.h" /* SIO32 hardware and driver information*/ #include "sio32Server.h" /* Sio32 server communication services */ #include "sio32Client.h" /* Sio32 client communication services */ #include "microCmd.h" /* Microcontoller command functions */ #include "console.h" /* Console Defines */ #include "controlDM.h" /* Control System Defines */ #include "sensorsDm.h" /* */ #include "sensorDcon.h" /* Vehicle Sensors Definitions */ #include "bellyPack.h" /* Belly Pack Micro Definitions */ #include "microDm.h" /* Micro Data Manager Items & Functions */ #include "microdef.h" #include "vmeIbc.h" /* VME Ibc header for Compass */ #include "powerTypes.h" #define Boolean MBool /* Map microcontroller Boolean to MBool */ #include "applic.h" /* Microcontroller applications */ #include "microcmd.h" /* Microcontroller commands */ #include "bellypk.h" /* Belly Pack Microcontroller defines */ #include "microTask.h" /* Microcontroller Task IO definitions */ #include "kalman.h" /* Belly Pack Device Name */ MLocal char* const bellyPackName = "Belly Pack "; /* Belly Pack Data Mgr Item Name Prefix */ MLocal char* const bellyPackAppPrefix = BELLY_PACK_DM_PREFIX; /* Vehicle Sensor DM Item Name Prefix */ MLocal char* const sensorAppPrefix = SENSOR_DM_PREFIX; #define NUM_TASKS 3 /* Number of co-operating tasks */ #define BELLY_PACK_DM_PERIOD 50000/* Belly Pack Telemetery Update */ /* period 20 Hz*/ #define SENSOR_RATE 1000000/* Sensor Update rate 1 hz */ #define BELLY_PACK_IN_STACKSIZE 2300 /* Belly Pack Input Task size & TCB */ #define BELLY_PACK_OUT_STACKSIZE 2000 /* Belly Pack Output Task size & TCB */ #define JOYSTICK_X_FULL_SCALE 1024.0 /* Main Joystick X Axis Full Scale Value*/ #define JOYSTICK_Y_FULL_SCALE 1024.0 /* Main Joystick Y Axis Full Scale Value*/ #define JOYSTICK_Z_FULL_SCALE 1024.0 /* Depth Joystick Full Scale Value */ #define JOYSTICK_YAW_FULL_SCALE 1024.0 /* Main Joystick Yaw Full Scale Value */ /* Joystick Zero Offset Calibration */ #define JOYSTICK_X_OFFSET 516 /* Main Joystick X axis offset */ #define JOYSTICK_Y_OFFSET 506 /* Main Joystick Y axis offset */ #define JOYSTICK_Z_OFFSET 490 /* Depth Joystick axis offset */ #define JOYSTICK_YAW_OFFSET 511 /* Main Joystick Yaw axis offset */ /* Joystick Full Scale Calibration */ #define JOYSTICK_X_POS_FSV 931.0 /* Main Joystick positive X Max Value */ #define JOYSTICK_X_NEG_FSV 49.0 /* Main Joystick negative X Max Value */ #define JOYSTICK_Y_POS_FSV 994.0 /* Main Joystick positive Y Max Value */ #define JOYSTICK_Y_NEG_FSV 99.0 /* Main Joystick negative Y Max Value */ #define JOYSTICK_Z_POS_FSV 920.0 /* Depth Joystick positive Max Value */ #define JOYSTICK_Z_NEG_FSV 64.0 /* Depth Joystick negative Max Value */ #define JOYSTICK_YAW_POS_FSV 611.0 /* Main Joystick positive Yaw Max Value */ #define JOYSTICK_YAW_NEG_FSV 410.0 /* Main Joystick negative Yaw Max Value */ #define JOYSTICK_X_DEADBAND 30.0 /* Main Joystick Deadband region */ #define JOYSTICK_Y_DEADBAND 30.0 /* Main Joystick Deadband region */ #define JOYSTICK_Z_DEADBAND 30.0 /* DepthJoystick Deadband region */ #define JOYSTICK_YAW_DEADBAND 30.0 /* Yaw Joystick Deadband region */ #define KEEP_ALIVE_COUNT 3 /* Stop KA Packets after 1 second */ #define KEEP_ALIVE_TICKS KEEP_ALIVE_TIME typedef struct /* Belly Pack Data Manager Items */ { microDmItems microDm; /* Microcontroller Data Manager Items */ DM_Item enableReq; /* Belly Pack enable Request */ DM_Item enabled; /* Belly Pack Enabled Flag */ DM_Item bellyPackWaterAlarm; /* Housing Water Alarm Detected */ DM_Item bellyPackFanStallAlarm; /* Belly Pack Fan Status Alarm */ DM_Item autoHeading; /* Belly Pack Auto Heading Mode */ DM_Item autoDepth; /* Belly Pack Auto Depth Mode */ DM_Item joystickX; DM_Item joystickY; DM_Item joystickZ; DM_Item joystickYaw; } bellyPackDmItems; typedef struct /* Belly Pack Data Manager Items */ { DM_Item autoHeading; /* Belly Pack Auto Heading Mode */ DM_Item autoDepth; /* Belly Pack Auto Depth Mode */ DM_Item autoAltitude; DM_Item attitude; } controlDmItems; typedef struct { DM_Item depth; }vehicleDmItems; /****************************************************************************/ Void bellyPackInTask(bellyPackDmItems *dmItems, microIOControl *microIOCtrl); Void bellyPackOutTask(bellyPackDmItems* dmItems, controlDmItems* ctrlDmItems, vehicleDmItems* vehicDmItems, microIOControl *microIOCtrl); MLocal Word processBellyPackSRQ( Reg bellyPackDmItems* dmItems, Reg controlDmItems* ctrlDmItems, microIOControl *microIOCtrl ); MLocal STATUS bellyPackDmItemInit( bellyPackDmItems* dmItems, sio32Chan* bellyPackDataChan ); /****************************************************************************/ /* Function : updateWaterAlarmsDm */ /* Purpose : Read Belly Pack Water Alarm status & updates DM */ /* Inputs : Sio32 chan & water alarm status data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateWaterAlarmsDm( sio32Chan *bellyPackDataChan, DM_Item bellyPackWaterAlarmDm ) { Word bellyPackWaterAlarm; /* Belly Pack Water Alarm Status */ Int16 cmdStatus; /* Command Status */ Word reply[2]; /* Command Result from microcontroller */ /* Read Water Alarm Status */ if ( sendMicroCommand( bellyPackDataChan, &reply, sizeof(Word) * 2, 1, BELLY_PACK_APP | GET_WATER_ALARM ) == 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 Alarm */ wordsFromBuf(reply, 2, &cmdStatus, &bellyPackWaterAlarm ); /* Map from a word to a boolean */ bellyPackWaterAlarm = (bellyPackWaterAlarm ? TRUE : FALSE); /* Write water alarm status to Dmgr */ writeDmItem(bellyPackWaterAlarmDm, &bellyPackWaterAlarm, sizeof(bellyPackWaterAlarm)); } /* updateWaterAlarmsDm() */ /****************************************************************************/ /* Function : updateFanStallAlarmsDm */ /* Purpose : Read Belly Pack Fan Stall Alarm status & updates DM */ /* Inputs : Sio32 chan & fan Stall status data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateFanStallAlarmsDm( sio32Chan *bellyPackDataChan, DM_Item bellyPackFanStallAlarmDm ) { Word bellyPackFanStallAlarm; /* Belly Pack Fan Stall Alarm Status */ Int16 cmdStatus; /* Command Status */ Word reply[2]; /* Command Result from microcontroller */ /* Read Water Alarm Status */ if ( sendMicroCommand( bellyPackDataChan, &reply, sizeof(Word) * 2, 1, BELLY_PACK_APP | GET_FAN_STALL_ALARM ) == 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 Alarm */ wordsFromBuf(reply, 2, &cmdStatus, &bellyPackFanStallAlarm ); /* Map from a word to a boolean */ bellyPackFanStallAlarm = (bellyPackFanStallAlarm ? TRUE : FALSE); /* Write Fan Stall alarm status to Dmgr */ writeDmItem(bellyPackFanStallAlarmDm, &bellyPackFanStallAlarm, sizeof(bellyPackFanStallAlarm)); } /* updateFanStallAlarmsDm() */ /****************************************************************************/ /* Function : updateAutoHeadingDm */ /* Purpose : Read Belly Pack Auto Heading status & updates DM */ /* Inputs : Sio32 chan & Auto Heading status data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateAutoHeadingDm( sio32Chan *bellyPackDataChan, DM_Item bellyPackAutoHeadingDm ) { Word bellyPackAutoHeading; /* Belly Pack Auto HeadingStatus */ Int16 cmdStatus; /* Command Status */ Word reply[2]; /* Command Result from microcontroller */ /* Read Auto Heading Status */ if ( sendMicroCommand( bellyPackDataChan, &reply, sizeof(Word) * 2, 1, BELLY_PACK_APP | GET_AUTO_HEADING_MODE ) == 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 Mode */ wordsFromBuf(reply, 2, &cmdStatus, &bellyPackAutoHeading); /* Map from a word to a boolean */ bellyPackAutoHeading = (bellyPackAutoHeading ? TRUE : FALSE); /* Write Auto Heading status to Dmgr */ writeDmItem(bellyPackAutoHeadingDm, &bellyPackAutoHeading, sizeof(bellyPackAutoHeading)); } /* updateAutoHeadingDm() */ /****************************************************************************/ /* Function : writeHeading */ /* Purpose : Get Heading from dm and send to micro */ /* Inputs : Sio32 channel, Heading dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writeHeading( sio32Chan *bellyPackDataChan, DM_Item attitudeDm ) { VehicleAttitude attitude; /* Attitude Item */ Flt32 heading; /* Heading */ Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ Int16 roundedHeading; /* Read Wattage Thresholds from dm */ status = dm_read( attitudeDm, (char *) &attitude, sizeof(attitude), (DM_Time *) NULL); /* Extract heading from attitude struct */ heading = RADS_TO_DEGS(attitude.heading); /* Add 0.5 to round up after Int16 cast */ roundedHeading = ((Int16) (heading + 0.5)); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Send command to microcontroller */ if ( sendMicroCommand( bellyPackDataChan, &cmdStatus, sizeof(cmdStatus), 2, BELLY_PACK_APP | WRITE_HEADING, roundedHeading) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ return(ERROR); } /* writeHeading() */ /****************************************************************************/ /* Function : updateAutoDepthDm */ /* Purpose : Read Belly Pack Auto Depth status & updates DM */ /* Inputs : Sio32 chan & Auto Depth status data manager item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateAutoDepthDm( sio32Chan *bellyPackDataChan, DM_Item bellyPackAutoDepthDm ) { Word bellyPackAutoDepth; /* Belly Pack Auto Depth Status */ Int16 cmdStatus; /* Command Status */ Word reply[2]; /* Command Result from microcontroller */ /* Read Auto Depth Status */ if ( sendMicroCommand( bellyPackDataChan, &reply, sizeof(Word) * 2, 1, BELLY_PACK_APP | GET_AUTO_DEPTH_MODE ) == 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 Mode */ wordsFromBuf(reply, 2, &cmdStatus, &bellyPackAutoDepth ); /* Map from a word to a boolean */ bellyPackAutoDepth = (bellyPackAutoDepth ? TRUE : FALSE); /* Write Auto Depth status to Dmgr */ writeDmItem(bellyPackAutoDepthDm, &bellyPackAutoDepth, sizeof(bellyPackAutoDepth)); } /* updateAutoDepthDm() */ /****************************************************************************/ /* Function : writeDepth */ /* Purpose : Get Depth from dm and send to micro */ /* Inputs : Sio32 channel, Depth dm item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS writeDepth( sio32Chan *bellyPackDataChan, DM_Item depthDm ) { Flt64 depth; /* Depth */ Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ Int16 roundedDepth; /* Read Depth from dm */ status = dm_read( depthDm, (char *) &depth, sizeof(depth), (DM_Time *) NULL); if (depth < 0.0) /* Check for negative values */ depth = 0.0; /* Add .5 and cast to int16 to round up */ roundedDepth = ((Int16)(depth + 0.5)); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Send command to microcontroller */ if ( sendMicroCommand( bellyPackDataChan, &cmdStatus, sizeof(cmdStatus), 2, BELLY_PACK_APP | WRITE_DEPTH, roundedDepth ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ return(ERROR); } /* writeDepth() */ /*****************************************************************************/ /* Function : scaleJoystick */ /* Purpose : Scale raw joystick values into required range & add deadband*/ /* Inputs : Raw Joystick value, zero offset, desired full scale value, */ /* observed fullscale +ve and -ve values and deadband range */ /* Outputs : Returns scaled, adjusted joystick value */ /*****************************************************************************/ LOCAL Int16 scaleJoystick( Int16 joystickRaw, Int16 offset, Flt32 fullScaleActual, Flt32 fullScalePosObs, Flt32 fullScaleNegObs, Flt32 deadband ) { Flt32 joystickScaled; /* Adjusted joystick value to return */ joystickRaw -= offset; /* Subtract measured zero offset */ if (joystickRaw >= 0) /* Check quadrant of raw value */ { /* Scale raw value into desired range*/ joystickScaled = joystickRaw * fullScaleActual / (fullScalePosObs - offset); /* Limit scaled value to max limit */ if (joystickScaled > fullScaleActual) joystickScaled = fullScaleActual; /* Apply deadband range to joystick */ if (joystickScaled < deadband) joystickScaled = 0.0; } /* if */ else { /* Scale raw value into desired range*/ joystickScaled = joystickRaw * -fullScaleActual / (fullScaleNegObs - offset); /* Limit scaled value to max limit */ if (joystickScaled < - fullScaleActual) joystickScaled = - fullScaleActual; /* Apply deadband range to joystick */ if (joystickScaled > -deadband) joystickScaled = 0.0; } /* else */ /* Return scaled, adjusted value */ return( (Int16) joystickScaled); } /* scaleJoystick() */ /****************************************************************************/ /* Function : updateJoysticksDm */ /* Purpose : Read Joysticks and update data manager items */ /* Inputs : Sio32 channel, joystick item handles */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS updateJoysticksDm( sio32Chan *bellyPackDataChan, DM_Item joystickXDm, DM_Item joystickYDm, DM_Item joystickZDm, DM_Item joystickYawDm) { Int16 joystickX, joystickY, joystickZ, joystickYaw; /* Joystick values */ Int16 cmdStatus; /* Command Status */ Word reply[5]; /* Command Result from microcontroller */ /* Send command to microcontroller */ if ( sendMicroCommand( bellyPackDataChan, &reply, sizeof(Word) * 5, 1, BELLY_PACK_APP | GET_JOYSTICKS ) == 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 joystick value */ wordsFromBuf(reply, 5, &cmdStatus, &joystickX, &joystickY, &joystickZ, &joystickYaw); joystickX = scaleJoystick(joystickX, JOYSTICK_X_OFFSET, JOYSTICK_X_FULL_SCALE, JOYSTICK_X_POS_FSV, JOYSTICK_X_NEG_FSV, JOYSTICK_X_DEADBAND); joystickY = scaleJoystick(joystickY, JOYSTICK_Y_OFFSET, JOYSTICK_Y_FULL_SCALE, JOYSTICK_Y_POS_FSV, JOYSTICK_Y_NEG_FSV, JOYSTICK_Y_DEADBAND); joystickZ = scaleJoystick(joystickZ, JOYSTICK_Z_OFFSET, JOYSTICK_Z_FULL_SCALE, JOYSTICK_Z_POS_FSV, JOYSTICK_Z_NEG_FSV, JOYSTICK_Z_DEADBAND); joystickYaw = scaleJoystick(joystickYaw, JOYSTICK_YAW_OFFSET, JOYSTICK_YAW_FULL_SCALE, JOYSTICK_YAW_POS_FSV, JOYSTICK_YAW_NEG_FSV, JOYSTICK_YAW_DEADBAND); /* Negate Joystick Yaw Values reversed */ /* on BP Hardware */ joystickYaw = -joystickYaw; writeDmItem( joystickXDm, &joystickX, sizeof(joystickX) ); writeDmItem( joystickYDm, &joystickY, sizeof(joystickY) ); writeDmItem( joystickZDm, &joystickZ, sizeof(joystickZ) ); writeDmItem( joystickYawDm, &joystickYaw, sizeof(joystickYaw) ); return(OK); } /* updateJoysticksDm() */ /****************************************************************************/ /* Function : enabledLampControl */ /* Purpose : Get Enabled Lamp Mode from dm and send to micro */ /* Inputs : Sio32 channel, Enabled Lamp Mode */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS enabledLampControl( sio32Chan *bellyPackDataChan, DM_Item enableReqDm, DM_Item enabledDm ) { MBool enabled; /* Enabled Mode */ MBool enableReq; /* Enable Request */ Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ /* Read Enabled Mode from dm */ status = dm_read( enableReqDm, (char *) &enableReq, sizeof(enableReq), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Send command to microcontroller */ if ((sendMicroCommand( bellyPackDataChan, &cmdStatus, sizeof(cmdStatus), 2, BELLY_PACK_APP | ENABLED_LAMP_CONTROL, (Word) enableReq) == ERROR) || (wordFromBuf(&cmdStatus, 0) != CMD_OK)) { enabled = ! enableReq; urgentWriteDmItem(enabledDm, &enabled, sizeof(enabled)); return(ERROR); } /* if */ enabled = enableReq; urgentWriteDmItem(enabledDm, &enabled, sizeof(enabled)); /* Check command result */ return (OK); } /* if */ return(ERROR); } /* enabledLampControl() */ /****************************************************************************/ /* Function : autoDepthLampControl */ /* Purpose : Get Auto Depth Lamp Mode from dm and send to micro */ /* Inputs : Sio32 channel, Auto Depth Lamp Mode */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS autoDepthLampControl( sio32Chan *bellyPackDataChan, DM_Item autoDepthDm ) { MBool autoDepth; /* Auto Depth Mode */ Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ /* Read Auto Depth Mode from dm */ status = dm_read( autoDepthDm, (char *) &autoDepth, sizeof(autoDepth), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Send command to microcontroller */ if ( sendMicroCommand( bellyPackDataChan, &cmdStatus, sizeof(cmdStatus), 2, BELLY_PACK_APP | AUTO_DEPTH_LAMP_CONTROL, autoDepth ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ return(ERROR); } /* autoDepthLampControl() */ /****************************************************************************/ /* Function : autoHeadingLampControl */ /* Purpose : Get Auto Heading Lamp Mode from dm and send to micro */ /* Inputs : Sio32 channel, Auto Heading Lamp Mode */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS autoHeadingLampControl( sio32Chan *bellyPackDataChan, DM_Item autoHeadingDm ) { MBool autoHeading; /* Auto Heading Mode */ Errno status; /* Result of dm_read */ Word cmdStatus; /* Command Result from microcontroller */ /* Read Auto Heading Mode from dm */ status = dm_read( autoHeadingDm, (char *) &autoHeading, sizeof(autoHeading), (DM_Time *) NULL); /* Check for successful dm_read */ if ((status == SUCCESS) || (status == EDM_NOPROVIDER)) { /* Send command to microcontroller */ if ( sendMicroCommand( bellyPackDataChan, &cmdStatus, sizeof(cmdStatus), 2, BELLY_PACK_APP | AUTO_HEADING_LAMP_CONTROL, autoHeading ) == ERROR ) return(ERROR); /* Check command result */ return (wordFromBuf(&cmdStatus, 0) == CMD_OK ? OK : ERROR); } /* if */ return(ERROR); } /* autoHeadingLampControl() */ /****************************************************************************/ /* Function : createBellyPackAppDmItems */ /* Purpose : Create Data Mgr Items specific to Belly Pack Application */ /* Inputs : Pointer to structure of DM item handles, Belly Pack DM */ /* prefix, Belly Pack Microcontroller Name */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ LOCAL STATUS createBellyPackAppDmItems( bellyPackDmItems *dmItems, controlDmItems *ctrlDmItems, vehicleDmItems *vehicDmItems, const char* bellyPackAppPrefix, const char* sensorAppPrefix, const char* bellyPackName ) { dmItemList bellyPackDmInit[] = { { &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->bellyPackWaterAlarm, "Belly Pack Water Alarm", BELLY_PACK_WATER_ALARM_DM, DM_MBOOL, 1 }, { &dmItems->bellyPackFanStallAlarm, "Belly Pack Fan Stall Alarm", BELLY_PACK_FAN_STALL_ALARM_DM, DM_MBOOL, 1 }, { &dmItems->joystickX, "Joystick X", BELLY_PACK_JOYSTICK_X_DM, DM_INT16, 1 }, { &dmItems->joystickY, "Joystick Y", BELLY_PACK_JOYSTICK_Y_DM, DM_INT16, 1 }, { &dmItems->joystickZ, "Joystick Z", BELLY_PACK_JOYSTICK_Z_DM, DM_INT16, 1 }, { &dmItems->joystickYaw, "Joystick Yaw", BELLY_PACK_JOYSTICK_HEADING_DM, DM_INT16, 1 }, { &dmItems->enableReq, "Enable Request", BELLY_PACK_ENABLE_REQ_DM, DM_MBOOL, 1}, { &dmItems->enabled, "Enabled Switch", BELLY_PACK_ENABLED_DM, DM_MBOOL, 1 }, { &dmItems->autoDepth, "Auto Depth Switch", BELLY_PACK_AUTO_DEPTH_DM, DM_MBOOL, 1 }, { &dmItems->autoHeading, "Auto Heading Switch", BELLY_PACK_AUTO_HEADING_DM, DM_MBOOL, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; dmItemList controlDmInit[] = { { &ctrlDmItems->autoDepth, "Auto Depth Switch", AUTO_DEPTH_DM, DM_MBOOL, 1 }, { &ctrlDmItems->autoHeading, "Auto Heading Switch", AUTO_HEADING_DM, DM_MBOOL, 1 }, { &ctrlDmItems->autoAltitude, "Auto Altitude Switch", AUTO_ALTITUDE_DM, DM_MBOOL, 1 }, { &ctrlDmItems->attitude, "attitude ", ATTITUDE_DM, DM_FLT32, 6 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; dmItemList vehicleDmInit[] = { { &vehicDmItems->depth, "Depth ", DEPTH_METERS_DM, DM_FLT64, 1 }, { NO_ITEM, NULL, NULL, DM_ENDT, 0 } }; /* Initialize Belly Pack Data Manager Items*/ if (initMicroDmItemList( bellyPackDmInit, bellyPackAppPrefix, "Bellypack" ) != OK) return(ERROR); /* Initialize Control Data Manager Items */ if (initMicroDmItemList( controlDmInit, "", "Bellypack Control" ) != OK) return(ERROR); /* Initialize Vehicle Data Manager Items */ if (initMicroDmItemList( vehicleDmInit, sensorAppPrefix, "Bellypack Vehicle" ) != OK) return(ERROR); return(OK); } /* createBellyPackAppDmItems() */ /****************************************************************************/ /* Function : bellyPackDmItemInit */ /* Purpose : Read parameters from Belly Pack microcontroller and update */ /* DMgr */ /* Inputs : Pointer to DM item handles, BellyPack micro serial channel */ /* Outputs : None, but data manager items change */ /****************************************************************************/ LOCAL STATUS bellyPackDmItemInit(bellyPackDmItems* dmItems, sio32Chan* bellyPackDataChan) { Int16 trys; /* Send Command Enable Command to IBC Micro */ for (trys = 0; ((microCmdEnable(bellyPackDataChan) == ERROR) && trys < MICRO_ENABLE_TRYS); trys++) taskDelay(sysClkRateGet() / 5); if (trys == MICRO_ENABLE_TRYS) { logMsg("Belly Pack: Error Enabling Belly Pack Microcontroller\n"); return(ERROR); } /* Set Micro Identification initial value */ if (updateMicroIdentDm(bellyPackDataChan, dmItems->microDm.idString) == ERROR) logMsg("Belly Pack: Error reading Identification String\n"); /* Set Micro Serial Number initial value */ if (updateSerialNoDm(bellyPackDataChan, dmItems->microDm.serialNo) == ERROR) logMsg("Belly Pack: Error reading Micro Serial Number\n"); /* Set Software Revision initial value */ if (updateSoftwareRevDm(bellyPackDataChan, dmItems->microDm.softwareRev) == ERROR) logMsg("Belly Pack: Error reading Software Revision\n"); /* Set Micro Ram Test Result value */ if (updateMicroRamTestDm(bellyPackDataChan, dmItems->microDm.ramTest) == ERROR) logMsg("Belly Pack: Error checking micro RAM Test\n"); /* Set Micro Water Alarm initial value */ if (updateWaterAlarmsDm(bellyPackDataChan, dmItems->bellyPackWaterAlarm) == ERROR) logMsg("Belly Pack: Error checking water alarm status\n"); /* Set Fan Stall Alarm initial value */ if (updateFanStallAlarmsDm(bellyPackDataChan, dmItems->bellyPackFanStallAlarm) == ERROR) logMsg("Belly Pack: Error checking Fan Stall alarm status\n"); if (updateAutoHeadingDm(bellyPackDataChan, dmItems->autoHeading) == ERROR) logMsg("Belly Pack: Error checking Auto Heading status\n"); if (updateAutoDepthDm(bellyPackDataChan, dmItems->autoDepth) == ERROR) logMsg("Belly Pack: Error checking Auto Depth status\n"); /* Initialize Belly Pack Enabled to False */ writeBooleanDmItem(dmItems->enabled, FALSE); return(OK); } /* bellyPackDmItemInit() */ /****************************************************************************/ /* Function : bellyPackTask */ /* Purpose : Initializes communication with Belly Pack Microcontroller */ /* Provides interface between Belly Pack and Data Manager */ /* Inputs : SIO32 serial channel number for Belly Pack Microcontrller */ /* Physically maps the Belly Pack to a serial channel */ /* Outputs : Normally runs forever, but returns ERROR on fatal error */ /****************************************************************************/ STATUS bellyPackTask( Nat16 bellyPackSerialChan ) { microIOControl microIOCtrl; /* Microcontroller IO Task control */ bellyPackDmItems dmItems; /* Belly Pack Application Data Mgr items */ controlDmItems ctrlDmItems; vehicleDmItems vehicDmItems; /* Initialize serial channel and task */ /* synchronization resources */ if (initMicroTaskIO( µIOCtrl, NUM_TASKS, bellyPackSerialChan, bellyPackAppPrefix, "/sio32/bellyPack", NO_ITEM ) == ERROR) /* "TIBURON.CONSOLE.BELLY_PACK_SWITCH_STATUS" ) == ERROR) */ { logMsg("Error initializing IO for Belly Pack Micro"); return(ERROR); } /* if */ #if 0 sio32ChanSetRxTimeout(µIOCtrl.sio32DataChan, sysClkRateGet() * 5); #endif /* Clear Belly Pack Data Manager handles */ bzero(&dmItems, sizeof(dmItems)); /* Create Belly Pack Application Items */ if (createBellyPackAppDmItems(&dmItems, &ctrlDmItems, &vehicDmItems, bellyPackAppPrefix, sensorAppPrefix, bellyPackName) == ERROR) { /* Error occurred so print message & exit */ sio32ChanDestroy( bellyPackSerialChan, µ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.bellyPackWaterAlarm, DM_STATIC); dm_start_provider(dmItems.bellyPackFanStallAlarm, DM_STATIC); dm_start_provider(dmItems.autoDepth, DM_STATIC); dm_start_provider(dmItems.autoHeading, DM_STATIC); /* Initialize Belly Pack Data Out Task */ if ((microIOCtrl.childTasks[OUT_TASK].taskId = taskSpawn("bellyPackOut", BELLY_PACK_OUT_PRIORITY, VX_FP_TASK, BELLY_PACK_OUT_STACKSIZE, (FUNCPTR) bellyPackOutTask, (int) &dmItems, (int) &ctrlDmItems, (int) &vehicDmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0)) == ERROR) { /* Close serial communication channel */ sio32ChanDestroy( bellyPackSerialChan, µIOCtrl.sio32DataChan ); logMsg("Belly Pack: Error spawning data output task\n"); return(ERROR); } /* if */ /* Initialize Belly Pack Data In Task */ if (( microIOCtrl.childTasks[IN_TASK].taskId = taskSpawn( "bellyPackIn", BELLY_PACK_IN_PRIORITY, VX_FP_TASK, BELLY_PACK_IN_STACKSIZE, (FUNCPTR) bellyPackInTask, (int) &dmItems, (int) µIOCtrl, 0, 0, 0, 0, 0, 0, 0, 0) ) == ERROR) { /* Delete Sonar Dcon Out Task */ taskDelete(microIOCtrl.childTasks[OUT_TASK].taskId); sio32ChanDestroy( bellyPackSerialChan, µIOCtrl.sio32DataChan ); logMsg("Belly Pack: Error spawning data input task\n"); return(ERROR); } /* if */ FOREVER { if (microIOCtrl.resetEventCount != 0) /* Process SRQ's from micro */ while(processBellyPackSRQ(&dmItems, &ctrlDmItems, µIOCtrl) != MICRO_RESET_SRQ); microIOCtrl.resetEventCount++; /* Count reset events ffrom micro */ /* Reset event received from Micro so */ /* Mark serial coms link as up */ setSerialLinkState(µIOCtrl, LINK_UP); /* Initialize PDU Micro DM Item values */ if (bellyPackDmItemInit( &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 channel */ sio32ChanDestroy(bellyPackSerialChan, µ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 */ } /* bellyPackTask() */ /*****************************************************************************/ /* Function : bellyPackOutTask */ /* Purpose : Belly Pack Out Task. Read DM items & write to Belly Pack */ /* Inputs : Array of Data Manager Item handles & serial chan structure */ /* Outputs : Normally runs forever, but exits if error occurs */ /*****************************************************************************/ Void bellyPackOutTask(Reg bellyPackDmItems *dmItems, Reg controlDmItems *ctrlDmItems, Reg vehicleDmItems *vehicDmItems, microIOControl *microIOCtrl) { DM_Group dmGroup; /* Belly Pack Data Manager Group */ SEM_ID dmUpdateSem; /* Data Manager wakes up task when an */ DWord changedBits; /* Holds dm_group_changes bit vector */ DWord bellyPackSwitchBit; /* Belly Pack Power On Bit */ DWord resetEventBit; /* Microcontroller reset event bit */ DWord resetCmdBit; /* Microcontroller reset item changed bit*/ DWord enableReqBit; /* Enable Req bit */ DWord enabledBit; /* Belly Pack Enabled bit */ DWord depthBit; /* Depth item Changed Bit */ DWord attitudeBit; /* Attitude Item Changed Bit */ DWord autoHeadingBit; /* Auto Heading Item Changed Bit */ DWord autoDepthBit; /* Auto Depth Item Changed Bit */ /* Create switch & power sync semaphores */ if ((dmUpdateSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL) { logMsg("Belly Pack: Error initializing Input Task Resources\n"); return; } /* if */ /* Declare this task a consumer of items */ /* which are written to the Belly Pack */ /* This task consumes motor power switch */ #if 0 dm_start_consumer(microIOCtrl->powerSwitchDm, DM_STATIC, dmUpdateSem); #endif dm_start_consumer(dmItems->microDm.resetEvent, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->microDm.resetCmd, DM_STATIC, dmUpdateSem); dm_start_consumer(dmItems->enableReq, DM_STATIC, dmUpdateSem); dm_start_consumer(ctrlDmItems->autoDepth, DM_STATIC, dmUpdateSem); dm_start_consumer(ctrlDmItems->autoHeading, DM_STATIC, dmUpdateSem); dm_start_consumer(vehicDmItems->depth, SENSOR_RATE, dmUpdateSem); dm_start_consumer(ctrlDmItems->attitude, SENSOR_RATE, dmUpdateSem); dm_start_provider(dmItems->enabled, DM_STATIC); dmGroup = dm_create_group(); /* Create Data Manager Group for items */ #if 0 dm_group_add_item(dmGroup, microIOCtrl->powerSwitchDm, &bellyPackSwitchBit); #endif dm_group_add_item(dmGroup, dmItems->microDm.resetEvent, &resetEventBit); dm_group_add_item(dmGroup, dmItems->microDm.resetCmd, &resetCmdBit); dm_group_add_item(dmGroup, dmItems->enableReq, &enableReqBit); dm_group_add_item(dmGroup, vehicDmItems->depth, &depthBit); dm_group_add_item(dmGroup, ctrlDmItems->attitude, &attitudeBit); dm_group_add_item(dmGroup, ctrlDmItems->autoDepth, &autoDepthBit); dm_group_add_item(dmGroup, ctrlDmItems->autoHeading, &autoHeadingBit); FOREVER { microIOCtrl->microConfigured = FALSE; /* Disable Bellypack enable */ urgentWriteDmItem(dmItems->enabled, FALSE, sizeof(MBool)); /* Disable Bellypack enable */ /* 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 */ /* Clear item changed bits after reset */ changedBits = dm_get_group_changes(dmGroup); /* Flag that micro has been configured */ microIOCtrl->microConfigured = TRUE; /* Wake up input task to read from micro */ semGive(microIOCtrl->childTasks[IN_TASK].taskSyncSem); #if 0 /* Change Serial I/O Keep Alive Mode so */ /* bellyPack disables if comms stops */ if (sio32ChanSetKeepAliveMode(microIOCtrl->sio32DataChan.channel, KEEP_ALIVE_TICKS, KEEP_ALIVE_COUNT) == ERROR) logMsg("Error setting Keep Alive Mode for Belly Pack"); #endif while (microIOCtrl->serialLinkState == LINK_UP) { /* Wait for any data items to change */ semTake(dmUpdateSem, WAIT_FOREVER); /* Ask Data Manager which items changed */ changedBits = dm_get_group_changes(dmGroup); /* Check if micro Reset Event occurred */ if (changedBits & resetEventBit) break; /* Exit loop and re-initialize DM items */ #if 0 /* Check if Belly Pack power switched off*/ if ( (changedBits & bellyPackSwitchBit) && (microDevPowerSwitchState(microIOCtrl) != ON) ) { /* Pan/Tilt switched off so link is down */ setSerialLinkState(microIOCtrl, LINK_DOWN); if (sio32ChanSetKeepAliveMode( microIOCtrl->sio32DataChan.channel, 0, 0) == ERROR) logMsg("Error clearing Keep Alive Mode for Belly Pack"); break; /* Exit loop and wait for power up */ } /* if */ #endif taskSafe(); /* Prevent task from being deleted during*/ /* serial IO and Data Manager updates */ if (changedBits & depthBit) writeDepth(SERIAL_CHAN, vehicDmItems->depth); if (changedBits & attitudeBit) writeHeading(SERIAL_CHAN, ctrlDmItems->attitude); if (changedBits & enableReqBit) enabledLampControl(SERIAL_CHAN, dmItems->enableReq, dmItems->enabled); if (changedBits & autoDepthBit) autoDepthLampControl(SERIAL_CHAN, ctrlDmItems->autoDepth); if (changedBits & autoHeadingBit) autoHeadingLampControl(SERIAL_CHAN, ctrlDmItems->autoHeading); /* Check if Reset Command request occurred*/ if (changedBits & resetCmdBit) { /* Mark serial link as down */ setSerialLinkState(microIOCtrl, LINK_DOWN); #if 0 /* 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 Keep Alive Mode for Belly Pack"); #endif /* Manual reset so announce it */ microIOCtrl->announceResets = TRUE; /* Send Reset Command to Microcontroller */ microResetCmd(SERIAL_CHAN); break; /* Exit loop and wait for RESET */ } /* if */ taskUnsafe(); /* Task may now be safely deleted */ } /* while (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 ! */ } /* bellyPackOutTask() */ /*****************************************************************************/ /* Function : bellyPackInTask */ /* Purpose : Belly Pack Input Task. Wakes up periodically, read data */ /* from the Belly Pack 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 bellyPackInTask(Reg bellyPackDmItems* dmItems, microIOControl *microIOCtrl ) { MBool enabled; SEM_ID wakeupSem; /* Used by watch dog to wake up this task*/ /* Create wakeup semaphore and watch dog */ /* timer for provider rate control */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Belly Pack: Error initializing Input Task Resources\n"); return; } /* if */ /* Provider Task for non-static data items*/ dm_start_provider(dmItems->joystickX, BELLY_PACK_DM_PERIOD); dm_start_provider(dmItems->joystickY, BELLY_PACK_DM_PERIOD); dm_start_provider(dmItems->joystickZ, BELLY_PACK_DM_PERIOD); dm_start_provider(dmItems->joystickYaw, BELLY_PACK_DM_PERIOD); dm_start_consumer(dmItems->enabled, BELLY_PACK_DM_PERIOD, SEM_NULL ); /* Update Rate is not guaranteed due to */ /* the possibility of task preemption */ FOREVER { /* Wait for IBC Coms to be initialized */ semTake(microIOCtrl->childTasks[IN_TASK].taskSyncSem, WAIT_FOREVER); while (microIOCtrl->serialLinkState == LINK_UP) { taskSafe(); /* Prevent task from being deleted during*/ /* serial IO and Data Manager updates */ dm_read( dmItems->enabled, (char *) &enabled, sizeof(enabled), (DM_Time *) NULL); if (enabled) /* serial IO and Data Manager updates */ updateJoysticksDm(SERIAL_CHAN, dmItems->joystickX, dmItems->joystickY, dmItems->joystickZ, dmItems->joystickYaw); taskUnsafe(); /* task may now be deleted safely */ /* pend until sem timeout wakes us up */ semTake(wakeupSem, sysClkRateGet() / (USECS_PER_SEC / BELLY_PACK_DM_PERIOD)); } /* while */ } /* FOREVER */ semDelete(wakeupSem); /* semaphore resources */ /* dm_stop_provider, dm_stop_consumer, dm_delete_group*/ /* etc are not required when the task exits as the */ /* data manager does this automatically when the task */ /* is deleted via the taskDeleteHookAdd() and */ /* dm_task_exit functions. */ /* that's all folks ! */ } /* bellyPackInTask() */ /*****************************************************************************/ /* Function : processBellyPackSRQ */ /* 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 processBellyPackSRQ( Reg bellyPackDmItems* dmItems, Reg controlDmItems* controlDmItems, microIOControl *microIOCtrl) { 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 */ /* Block on Reading Service Request */ if (readSRQPacket(SERIAL_CHAN, buffer, SRQ_PACKET_LEN, WAIT_FOREVER) != 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); break; case WATER_ALARM_ON_SRQ: /* Housing Water Alarm Active */ writeBooleanDmItem(dmItems->bellyPackWaterAlarm, TRUE); break; case WATER_ALARM_OFF_SRQ: /* Housing Water Alarm Inactive */ writeBooleanDmItem(dmItems->bellyPackWaterAlarm, FALSE); break; case FAN_STALL_ALARM_ON_SRQ: /* Fan Stall Alarm Active */ writeBooleanDmItem(dmItems->bellyPackFanStallAlarm, TRUE); break; case FAN_STALL_ALARM_OFF_SRQ: /* Fan Stall Alarm Inactive */ writeBooleanDmItem(dmItems->bellyPackFanStallAlarm, FALSE); break; case AUTO_DEPTH_ON_SRQ: /* Auto Depth Active */ { urgentWriteDmItem( controlDmItems->autoAltitude, FALSE, sizeof(MBool)); writeBooleanDmItem(dmItems->autoDepth, TRUE); } break; case AUTO_DEPTH_OFF_SRQ: /* Auto Depth Inactive */ writeBooleanDmItem(dmItems->autoDepth, FALSE); break; case AUTO_HEADING_ON_SRQ: /* Auto Heading Active */ writeBooleanDmItem(dmItems->autoHeading, TRUE); break; case AUTO_HEADING_OFF_SRQ: /* Auto Heading Inactive */ writeBooleanDmItem(dmItems->autoHeading, FALSE); break; default: printf("Belly Pack: Unrecognized SRQ 0x%x\n", srqRequest); } /* switch */ return (srqRequest); /* Return SRQ to Caller */ } /* processBellyPackSRQ() */ /****************************************************************************/ /* Function : writeDisplays */ /* Purpose : Writes to BellyPack Display */ /* Inputs : Sio32 channel */ /* Outputs : Returns OK or ERROR */ /****************************************************************************/ STATUS writeDisplays( sio32Chan *serialChan, Byte *writeBuffer, Int16 nBytes ) { Word reply; Byte Packet[MAX_PACKET_LEN]; /* Command Result from microcontroller */ nBytes = min(nBytes, MAX_PACKET_LEN - sizeof(Word) * 4); wordsToBuf(Packet, 2, BELLY_PACK_APP | WRITE_DISPLAYS, nBytes); bcopy( writeBuffer, Packet + sizeof(Word) * 4, nBytes); return (microWriteBufReply( serialChan, nBytes + sizeof(Word) * 4, Packet)); } /* writeDisplays() */