/****************************************************************************/ /* Copyright 1995 to 1999 MBARI */ /****************************************************************************/ /* Summary : USBL Rovnav Acoustic Telemetry Software for vxWorks */ /* Filename : rovnav.c */ /* Author : Andrew Pearce */ /* Project : Tiburon */ /* Version : Version 1.0 */ /* Created : 09/09/97 */ /* Modified : 04/15/99 */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /* $Header: * $Log: */ /****************************************************************************/ #include /* vxWorks system declarations */ #include /* vxWorks semaphore functions */ #include /* vxWorks IO system */ #include /* vxWorks standard I/O functions */ #include /* vxWorks Message Queue Library */ #include "mbariTypes.h" /* MBARI style guide type declarations */ #include "mbariConst.h" /* Miscellaneous constants */ #include "rovPriority.h" /* MBARI Rov Application Priorities */ #include "usrTime.h" /* MBARI time and date functions */ #include "datamgr.h" /* Data Manager declarations */ #include "dm_errno.h" /* Data Manager error declarations */ #include "tiburon.h" /* Tiburon definitions */ #include "rovnav.h" /* ROVNAV Emergency Functions */ #include "emergency.h" #include "ersDcon.h" #include "sensorDcon.h" #include "ers.h" #define USBL_DEVICE "/tyCo/1" Int32 usblFd; /* USBL Serial Port Fd */ #define ROVNAV_DEBUG typedef struct { DM_Item emergencyOnlineDm; /* TRUE when emergency functions are OK */ DM_Item rovnavCmdDm; /* ROVNAV Cmd DM Item */ DM_Item testComsLinkDm; /* Tests Acoustic Modem Coms Link */ DM_Item comsLinkStatusDm; /* Result of Modem Coms Link */ DM_Item floodVbArmDm; /* Flood VB Emergency Arm DM Item */ DM_Item floodVbRunDm; /* Flood VB Emergency Cmd DM Item */ DM_Item reportVbLevelDm; /* Report VB Level */ DM_Item vbLevelMMDm; /* VB System Level (mm) */ DM_Item reportERSBatteryDm; /* Report VB Battery Life Remaining */ DM_Item ERSBatteryDm; /* ERS Battery Life Remaining */ DM_Item reportERSDepthDm; /* Report ERS Depth */ DM_Item ERSDepthDm; /* ERS Depth Sensor Value (meters) */ DM_Item pressureOffsetDm; /* Pressure Sensor Offset */ } ersDmItems; /****************************************************************************/ /* Function : sendRovnavPPMCmd */ /* Purpose : Send ROVNAV PPM Command to USBL for telemetry to Rovnav */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal STATUS sendRovnavPPMCmd(char *rovnavCmd) { char *cmdString = ROVNAV_PPM_FILL_STRING ROVNAV_TERM; sprintf(cmdString + 1, "%2x", rovnavCmd[2]); if (write (usblFd, ROVNAV_ADDR_STRING ROVNAV_TERM, strlen(ROVNAV_ADDR_STRING ROVNAV_TERM)) == ERROR) return (ERROR); if (write (usblFd, cmdString, strlen(cmdString)) == ERROR) return (ERROR); return(OK); } /* sendRovnavPPMCmd() */ /****************************************************************************/ /* Function : sendRovnavFSKCmd */ /* Purpose : Send ROVNAV FSK Command to USBL for telemetry to Rovnav */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal STATUS sendRovnavFSKCmd(char *rovnavCmd) { char *cmdString = ROVNAV_FSK_FILL_STRING ROVNAV_TERM; Byte cksum; cksum = 0xAA ^ rovnavCmd[0] ^ rovnavCmd[1] ^ rovnavCmd[2]; sprintf(cmdString + 7, "%2x%2x%2x%2x", rovnavCmd[0], rovnavCmd[1], rovnavCmd[2], cksum); if (write (usblFd, ROVNAV_ADDR_STRING ROVNAV_TERM, strlen(ROVNAV_ADDR_STRING ROVNAV_TERM)) == ERROR) return (ERROR); if (write (usblFd, cmdString, strlen(cmdString)) == ERROR) return (ERROR); if (write (usblFd, ROVNAV_FSK_CMD_STRING ROVNAV_TERM, strlen(ROVNAV_FSK_CMD_STRING ROVNAV_TERM)) == ERROR) return (ERROR); return(OK); } /* sendRovnavFSKCmd() */ MLocal STATUS readRovnavReply( char *reply, Int16 nBytes, Int16 timeout ) { Int32 bytesRead, tries; if (timeout) /* Timeout in seconds */ { tries = 0; bytesRead = 0; while ( (bytesRead < nBytes) && (tries++ < 10) ); { ioctl(usblFd, FIONREAD, (int) &bytesRead); if (bytesRead < nBytes) taskDelay(sysClkRateGet() / 10); } /* while */ if (tries == 11) return (ERROR); } /* if */ return (read(usblFd, reply, nBytes)); } /* readRovnavReply() */ /****************************************************************************/ /* Function : sendRovnavCmd */ /* Purpose : Send Serial Command to USBL for telemetry to Rovnav/ERS */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ MLocal STATUS sendRovnavCmd( rovnavTelemMode telemMode, char *rovnavCmd) { if (telemMode == ROVNAV_PPM_MODE) return (sendRovnavPPMCmd(rovnavCmd)); if (telemMode == ROVNAV_FSK_MODE) return (sendRovnavFSKCmd(rovnavCmd)); return (ERROR); } /* sendRovnavCmd() */ MLocal Void initRovnavDmItems( ersDmItems *dmItems ) { dmItems->emergencyOnlineDm = initMicroDmItem(TIBURON_DM_PREFIX, EMERGENCY_TELEM_ONLINE_DM, DM_MBOOL, 1); dmItems->testComsLinkDm = initMicroDmItem(TIBURON_DM_PREFIX, TEST_ERS_COMS_LINK_CMD_DM, DM_EMPTY, 1); dmItems->comsLinkStatusDm = initMicroDmItem(TIBURON_DM_PREFIX, ERS_COMS_LINK_STATUS_DM, DM_MBOOL, 1); dmItems->floodVbArmDm = initMicroDmItem(TIBURON_DM_PREFIX, FLOOD_VB_EMERGENCY_ARM_CMD_DM, DM_MBOOL, 1); dmItems->floodVbRunDm = initMicroDmItem(TIBURON_DM_PREFIX, FLOOD_VB_EMERGENCY_RUN_CMD_DM, DM_EMPTY, 1); dmItems->reportVbLevelDm = initMicroDmItem(TIBURON_DM_PREFIX, REPORT_ERS_VB_LEVEL_CMD_DM, DM_EMPTY, 1); dmItems->vbLevelMMDm = initMicroDmItem(TIBURON_DM_PREFIX, EMERGENCY_ERS_VB_LEVEL_MM_DM, DM_FLT32, 1); dmItems->reportERSBatteryDm = initMicroDmItem(TIBURON_DM_PREFIX, REPORT_ERS_BATTERY_CMD_DM, DM_EMPTY, 1); dmItems->ERSBatteryDm = initMicroDmItem(TIBURON_DM_PREFIX, EMERGENCY_ERS_BATTERY_LIFE_DM, DM_FLT32, 1); dmItems->reportERSDepthDm = initMicroDmItem(TIBURON_DM_PREFIX, REPORT_ERS_DEPTH_CMD_DM, DM_EMPTY, 1); dmItems->ERSDepthDm = initMicroDmItem(TIBURON_DM_PREFIX, EMERGENCY_ERS_DEPTH_METERS_DM, DM_FLT64, 1); dmItems->rovnavCmdDm = initMicroDmItem(ERS_DCON_DM_PREFIX, ERS_ROVNAV_CMD_DM, DM_CHAR, DM_CHAR, ROVNAV_CMD_LEN + 2); dmItems->pressureOffsetDm = initMicroDmItem(SENSOR_IBC_DM_PREFIX PRESSURE_SENSOR_OFFSET_DM, DM_FLT64, 1); } /* intErsDmItems() */ Void testERSComsLink( ersDmItems *dmItems ) { Char rovnavReply[ROVNAV_BUF_LEN]; MBool comsLinkStatus = FALSE; #ifdef ROVNAV_DEBUG printf ("Sending Test ERS Coms Link Command\n"); #endif sendRovnavCmd(ROVNAV_FSK_MODE, TEST_ERS_COMS_LINK_CMD); if (readRovnavReply(rovnavReply, 4, 10) == ERROR) return; #ifdef ROVNAV_DEBUG printf ("Received %s from Rovnav\n", rovnavReply); #endif comsLinkStatus = (strcmp(rovnavReply, ROVNAV_COMS_TEST_STRING) == EQUAL ? TRUE : FALSE); writeBooleanDmItem(dmItems->comsLinkStatusDm, comsLinkStatus); } /* testERSComsLink() */ Void reportVbLevelCmd( ersDmItems *dmItems ) { Flt32 vbLevelMM; /* VB System Level Millimeters */ Char rovnavReply[ROVNAV_BUF_LEN]; #ifdef ROVNAV_DEBUG printf ("Sending Report VB Level Command\n"); #endif sendRovnavCmd(ROVNAV_PPM_MODE, REPORT_VB_LEVEL_CMD); if (readRovnavReply(rovnavReply, 4, 10) == ERROR) return; #ifdef ROVNAV_DEBUG printf ("Received %s from Rovnav\n", rovnavReply); #endif /* Convert to Millimeters */ vbLevelMM = ((Flt32) atoi(rovnavReply)) / 10.0; writeDmItem(dmItems->vbLevelMMDm, &vbLevelMM, sizeof(vbLevelMM)); } /* reportVbLevelCmd() */ Void reportERSBatteryCmd( ersDmItems *dmItems ) { Flt32 batteryLifeAmpHours; Char rovnavReply[ROVNAV_BUF_LEN]; #ifdef ROVNAV_DEBUG printf ("Sending Report ERS Battery Life Command\n"); #endif sendRovnavCmd(ROVNAV_PPM_MODE, REPORT_ERS_BATTERY_CMD); if (readRovnavReply(rovnavReply, 4, 10) == ERROR) return; #ifdef ROVNAV_DEBUG printf ("Received %s from Rovnav\n", rovnavReply); #endif batteryLifeAmpHours = ((Flt32) atoi(rovnavReply)) / 10.0; writeDmItem(dmItems->ERSBatteryDm, &batteryLifeAmpHours, sizeof(batteryLifeAmpHours)); } /* reportERSBatteryCmd() */ Void reportERSDepthCmd( ersDmItems *dmItems ) { Flt64 pressure, depth; /* Raw value ERS */ Flt64 pressureOffset; /* Surface pressure offset */ Char rovnavReply[ROVNAV_BUF_LEN]; #ifdef ROVNAV_DEBUG printf ("Sending Report ERS Depth Command\n"); #endif sendRovnavCmd(ROVNAV_PPM_MODE, REPORT_ERS_PRESSURE_CMD); if (readRovnavReply(rovnavReply, 4, 10) == ERROR) return; #ifdef ROVNAV_DEBUG printf ("Received %s from Rovnav\n", rovnavReply); #endif dm_read(dmItems->pressureOffsetDm, &pressureOffset, sizeof(pressureOffset), (DM_Time *) NULL); pressure -= pressureOffset; /* Subtract surface pressure offset */ /* Convert pressure to meteres */ depth = pressure * DECIBARS_TO_METERS; writeDmItem(dmItems->ERSDepthDm, (char *) &depth, sizeof(depth)); } /* reportERSDepthCmd() */ /****************************************************************************/ /* Function : rovnavTask */ /* Purpose : Processes emergency telemetry commands for ERS */ /* Inputs : None */ /* Outputs : None */ /****************************************************************************/ STATUS rovnavTask( Void ) { SEM_ID wakeupSem; /* Used wake up this task periodically */ DM_Group dmGroup; /* Rovnav Data Manager Item Group */ ersDmItems dmItems; DWord changeBits; /* Holds dm_group_changes bit vector */ DWord testComsLinkBit; DWord floodVbArmBit; DWord floodVbRunBit; DWord reportVbLevelBit; DWord reportERSBatteryBit; DWord reportERSDepthBit; /* Set this task priority */ taskPrioritySet(taskIdSelf(), ROVNAV_TASK_PRIORITY); initRovnavDmItems( &dmItems ); dm_start_provider(dmItems.emergencyOnlineDm, DM_STATIC); writeBooleanDmItem(dmItems.emergencyOnlineDm, FALSE); if ((usblFd = open(USBL_DEVICE, O_RDWR, 0)) == ERROR) { printf ("USBL: Unable to open serial port %s\n", USBL_DEVICE); return (ERROR); } /* if */ /* Create wakeup semaphore */ if ((wakeupSem = semBCreate(SEM_Q_FIFO, SEM_EMPTY)) == NULL ) { logMsg("Rovnav Task: Error creating semaphore\n"); return(ERROR); } /* if */ dm_start_provider(dmItems.vbLevelMMDm, DM_STATIC); dm_start_provider(dmItems.ERSBatteryDm, DM_STATIC); dm_start_provider(dmItems.ERSDepthDm, DM_STATIC); dm_start_provider(dmItems.comsLinkStatusDm, DM_STATIC); writeBooleanDmItem(dmItems.comsLinkStatusDm, FALSE); dm_start_consumer(dmItems.testComsLinkDm, DM_STATIC, wakeupSem); dm_start_consumer(dmItems.floodVbArmDm, DM_STATIC, wakeupSem); dm_start_consumer(dmItems.floodVbRunDm, DM_STATIC, wakeupSem); dm_start_consumer(dmItems.reportVbLevelDm, DM_STATIC, wakeupSem); dm_start_consumer(dmItems.reportERSBatteryDm, DM_STATIC, wakeupSem); dm_start_consumer(dmItems.reportERSDepthDm, DM_STATIC, wakeupSem); dm_start_consumer(dmItems.pressureOffsetDm, DM_STATIC, SEM_NULL); dmGroup = dm_create_group(); /* Create DM Group for rovnav Application*/ dm_group_add_item(dmGroup, dmItems.testComsLinkDm, &testComsLinkBit); dm_group_add_item(dmGroup, dmItems.floodVbArmDm, &floodVbArmBit); dm_group_add_item(dmGroup, dmItems.floodVbRunDm, &floodVbRunBit); dm_group_add_item(dmGroup, dmItems.reportVbLevelDm, &reportVbLevelBit); dm_group_add_item(dmGroup, dmItems.reportERSBatteryDm, &reportERSBatteryBit); dm_group_add_item(dmGroup, dmItems.reportERSDepthDm, &reportERSDepthBit); /* Clear data manager item changed bits */ changeBits = dm_get_group_changes(dmGroup); writeBooleanDmItem(dmItems.emergencyOnlineDm, TRUE); FOREVER { semTake(wakeupSem, sysClkRateGet() * 60); changeBits = dm_get_group_changes(dmGroup); urgentWriteDmItem(dmItems.rovnavCmdDm, (void *) " ", 1); if (changeBits & testComsLinkBit) testERSComsLink( &dmItems ); if (changeBits & floodVbArmBit) { #ifdef ROVNAV_DEBUG printf ("Sending Arm VB Flood Emergency Command\n"); #endif sendRovnavCmd(ROVNAV_PPM_MODE, FLOOD_VB_ARM_CMD); } /* if */ if (changeBits & floodVbRunBit) #ifdef ROVNAV_DEBUG printf ("Sending Flood VB Emergency Command\n"); #endif sendRovnavCmd(ROVNAV_FSK_MODE, FLOOD_VB_ARM_CMD); } /* if */ if (changeBits & reportVbLevelBit) reportVbLevelCmd( &dmItems ); if (changeBits & reportERSBatteryBit) reportERSBatteryCmd( &dmItems ); if (changeBits & reportERSDepthBit) reportERSDepthCmd( &dmItems ); } /* FOREVER */ } /* rovnavTask() */