//////////////////////////////////////////////////////////////////////////////// // // PURPOSE: Dvl Driver. // AUTHOR: Rob McEwen, using code from Tom O'Reilly, Janice Tarrant, and // Don Green. // DATE: 01/3/9 // COMMENTS: This is the code that talks to the instrument. // //////////////////////////////////////////////////////////////////////////////// // #include #include "Dvl.h" #include "WorkSiteIF.h" #include "DvlOutput.h" #include "Syslog.h" #include "matrixMath.h" // // The dvl will be in hex/ascii mode, NOT binary mode. The output format still // will match Figure D-12 & 13 on pp. D-12 & 13 of the manual, except there // will be twice as many bytes, because each hex char is a byte. See also // p D-24, and note the first column. // #define MaxRecordBytes 176 //88 bytes = 176 hex chars. #define RecordTerminator "\r" #define ReadTimeout 2000 #define MaxInitTries 2 // // Initialize the direction cosine matrix that takes a vector from RDI ship // coordinates to SNAME standard ship coordinates. See my handwritten notes // on page C-26 of the RDI manual. // // I didn't make this a const because that will conflict with the type // declaration in the argument list of the C function that I pass it to. // double T_SNAME_RDI[NX][NX] = { { 0., 1., 0.}, { 1., 0., 0.}, { 0., 0.,-1.}, }; Dvl::Dvl(SerialDevice *device, Boolean verbose) : SerialDeviceDriver("Dvl", device, MaxRecordBytes, RecordTerminator, ReadTimeout, MaxInitTries) { m_log = new DvlLog(this, DataLog::BinaryFormat); m_output = new DvlOutput(); // // The object workSite is local to this constructor. // WorkSiteIF workSite("workSite"); m_sos = (int) workSite.soundSpeed(); // // Initialize pointers. This is done to avoid editing long names // into the pre-existing DVL code. // #if USE_POINTERS bottomTrackVelocity = m_output->data.bottomTrackVelocity; //4 element vector. waterMassVelocity = m_output->data.waterMassVelocity; //4 element vector. bottomStatus = &(m_output->data.bottomStatus); waterStatus = &(m_output->data.waterStatus); dvlPingTime = &(m_output->data.pingTime); dvlTemp = &(m_output->data.temp); dvlRange = &(m_output->data.range); dvlPitch = &(m_output->data.pitch); dvlRoll = &(m_output->data.roll); dvlHeading = &(m_output->data.heading); #endif m_verbose = verbose; m_timeout = 2000; //milli-seconds // // Initialize badComms to 0. // m_output->data.badComms = 0; } Dvl::~Dvl() { Boolean debug = m_verbose; char cmd[32]; char cmdnr[32]; delete m_log; delete m_output; wakeupDvl(); // Stop it from pinging // // Power down. // sprintf(cmd, "CZ\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { // // The dvl doesn't echo a ">" after the CS command. // dprintf(" Dvl::Destructor: %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::Destructor: _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } Syslog::write(" Dvl::~Dvl: Destructor Finished.\n"); } //////////////////////////////////////////////////////////////////////////////// // // Initialize the Dvl. This routine is apparently called by SerialDeviceDriver's // constructor. // // NOTE: _device is a member of the class SerialDeviceDriver, which this // inherits from. // //////////////////////////////////////////////////////////////////////////////// // DeviceIF::Status Dvl::initialize() { Boolean debug = m_verbose; Syslog::write("\n Starting DVL Initialization.\n"); DeviceIF::Status status = DeviceIF::Ok; // // Set up the serial line format: // if(_device->setLineFormat(BAUD_RATE,DATA_BITS,STOP_BITS,PARITY) == ERROR ) { Syslog::write(" DVL::initialize: ERROR - Serial Port Formatting\n"); return DeviceIF::Error; } // // Send a 300 ms Tx ON to get it's attention: // wakeupDvl(); try{_device->clearPort();} catch( Exception errorObject ) { Syslog::write("Dvl::initialize() clearPort() %s", errorObject.msg ); } // // Have the Dvl spew back it's parameters, just for the record. // char cmd[100]; char cmdnr[100]; char buf[1028]; sprintf(cmd, "PS0\r"); if( m_verbose ) { if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): PS0 timed out. %d bytes read.\n", errorObject.nBytesRead() ); Syslog::write(buf); return status; } Syslog::write("Dvl::initialize():"); Syslog::write(buf); Syslog::write(" Dvl::initialize(): Received %d bytes.\n", strlen(buf) ); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(PS0) didn't work.\n"); status = DeviceIF::Error; } } //if( m_verbose ) // // Reset the DVL to the factory settings. // sprintf(cmd, "CR1\r"); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): CR1 timed out. %d bytes read.\n", errorObject.nBytesRead() ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): CR1 Command sent.\n"); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(CR1) didn't work.\n"); status = DeviceIF::Error; } // // Set BK1, which sends a water-mass layer after every bottom-track ping. // We should consider setting this to BK2 for operational use, which would // send a water-mass layer ping only if the bottom-track ping doesn't return. // This presumably would save power, and decrease the sampling period. // sprintf(cmd, "BK1\r"); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): BK2 command timed out. %d bytes read.\n", errorObject.nBytesRead() ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): BK2 Command sent.\n"); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(BK2) didn't work.\n"); status = DeviceIF::Error; } // // Set the water-tracking layer to 10 meters in depth, beginning from the // vehicle and extending 10 m down. // sprintf(cmd, "BL,100,000,100\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): %s Command timed out. " "%d bytes read.\n", errorObject.nBytesRead(), cmdnr ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } // // Consider setting BM to 4 here. BM defaults to 5, which is better for // shallow water. // // // Set BP001, which selects only one ping per ensemble. // sprintf(cmd, "BP001\r"); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): BP001 Command timed out. " "%d bytes read.\n", errorObject.nBytesRead() ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): BP001 Command sent.\n"); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(BP001) didn't work.\n"); status = DeviceIF::Error; } // // Set the minimum time between pings to be .2 sec, or 5 Hz. The Dvl will // in fact run slower, probably at around 2 Hz. // sprintf(cmd, "TP00:00.20\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): %s Command timed out. " "%d bytes read.\n", errorObject.nBytesRead(), cmdnr ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } // // Select PD5 output format. // sprintf(cmd, "PD5\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): %s Command timed out. " "%d bytes read.\n", errorObject.nBytesRead(), cmdnr ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } // // Select hex/ascii format (not binary). // sprintf(cmd, "CF11010\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): %s Command timed out. " "%d bytes read.\n", errorObject.nBytesRead(), cmdnr ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } // // Set the speed of sound. // sprintf(cmd, "EC%d\r", m_sos ); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): %s Command timed out. " "%d bytes read.\n", errorObject.nBytesRead(), cmdnr ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } // // Select Ship Coordinates. Do not apply the Pitch/Roll from the tilt // sensors in the coordinate transformation. Allow 3-beam solutions. // Turn on bin mapping, although it really doesn't matter since the // tilt sensors aren't applied. // sprintf(cmd, "EX10011\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): %s Command timed out. " "%d bytes read.\n", errorObject.nBytesRead(), cmdnr ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } // // Set the mounting angle to 45 Deg. This means that the alignment groove // on the DVL head should be coincident with the vehicle centerline, and // the alignment notch should be forward. This places the number 3 beam // forward and to starboard. // sprintf(cmd, "EA+04500\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { try{ _device->readUntil( buf, sizeof(buf), ">", m_timeout ); } catch( SerialDevice::TimedOut errorObject ) { status = DeviceIF::Error; Syslog::write("Dvl::initialize(): %s Command timed out. " "%d bytes read.\n", errorObject.nBytesRead(), cmdnr ); Syslog::write(buf); return status; } dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } // // Start it pinging: // sprintf(cmd, "CS\r"); strncpy(cmdnr, cmd, strlen(cmd)-1 ); //Get rid of the \r. cmdnr[strlen(cmd)-1] = '\0'; //Add the terminating null if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { // // The dvl doesn't echo a ">" after the CS command. // dprintf(" Dvl::initialize(): %s Command sent.\n", cmdnr); } else { // // The _device->write() didn't work // Syslog::write("Dvl::initialize(): _device->write(%s) didn't work.\n", cmdnr); status = DeviceIF::Error; } Syslog::write(" Dvl::initialize() - The DVL is initialized.\n"); return status; } // //////////////////////////////////////////////////////////////////////////////// // // Send a 300 ms Tx ON to wake up the Dvl. // //////////////////////////////////////////////////////////////////////////////// // DeviceIF::Status Dvl::wakeupDvl() { Boolean debug = m_verbose; // // This routine was modified from Don Green's code. // dprintf("Dvl::wakeupDvl: Begin\n"); // // A break to a RD Instruments Workhorse is a 300ms Tx High // int count = 1; Boolean tryagain = True; while((count<15) && (tryagain)) { dprintf("Attempting to interrupt the DVL; try number %d", count ); if(_device->sendBreak() == ERROR) { Syslog::write("Dvl::wakeupDvl: Error sending break signal to the DVL"); return DeviceIF::Error; } // // confirm reply // allow for long wait // try { if( _device->confirm(">",3*m_timeout) == ERROR) { Syslog::write(" Dvl::wakeupDvl: Failed to establish comms with DVL"); count++; tryagain = True; int fd = _device->getFd(); tcflush(fd,TCIFLUSH); } else { tryagain = False; } } catch(SerialDevice::TimedOut e) { dprintf(" Dvl::wakeupDvl: Caught a Timeout from SerialDevice"); count++; tryagain = True; int fd = _device->getFd(); tcflush(fd,TCIFLUSH); } catch(Exception e) { dprintf(" Dvl::wakeupDvl: Caught %s", e.msg ); count++; tryagain = True; int fd = _device->getFd(); tcflush(fd,TCIFLUSH); } catch(...) { dprintf(" Dvl::wakeupDvl: Caught an unknown exception"); count++; tryagain = True; int fd = _device->getFd(); tcflush(fd,TCIFLUSH); } } dprintf(" Dvl::wakeupDvl: End\n"); if(tryagain == True) return DeviceIF::Error; else return DeviceIF::Ok; } // //////////////////////////////////////////////////////////////////////////////// // // This routine overrides the readRecord declared in the base class, because // we don't get a terminating character. We just look for 176 bytes each time. // //////////////////////////////////////////////////////////////////////////////// // DeviceIF::Status Dvl::readRecord(unsigned char *record, int maxRecordBytes, const char *recordTerminator, unsigned readTimeout, int *nBytesRead) { Boolean debug = m_verbose; Boolean readError = False; try { // // Read data record from device // *nBytesRead = _device->readNChars((char *)record, maxRecordBytes, readTimeout); } catch (SerialDevice::TimedOut) { readError = True; m_output->data.badComms = 1; Syslog::write("%s::readRecord() - Dvl serial device timed out", name()); Syslog::write("%s::readRecord() - record so far: %s", name(), record); } catch (SerialDevice::BufferFull) { readError = True; m_output->data.badComms = 1; Syslog::write("%s::readRecord() - Dvl serial device buffer full", name()); } catch (Exception e) { readError = True; m_output->data.badComms = 1; m_output->write(); Syslog::write("%s::readRecord() - caught exception: abort", name()); throw; } dprintf("Dvl::readRecord(): %d bytes read.\n", *nBytesRead ); if( *nBytesRead != maxRecordBytes ) { dprintf( "Dvl::readRecord(): Incorrect number of bytes read.\n"); m_output->data.badComms = 1; m_output->write(); return DeviceIF::Error; } // // Compute the checksum for the record that we just read in: // short byte; long byteVal; long checksum = 0; long advChecksum, checkSumLsb, checkSumMsb; for (byte = 0; byte < (*nBytesRead-4); byte+=2) { sscanf( (char *)( record + byte ), "%2x", &byteVal ); checksum += byteVal; } dprintf(" %s::readRecord() - checksum = %d\n", name(), checksum ); // // Now, read in the checksum that the DVL computed, and compare with the // one that we computed here. // sscanf( (char *) (record + *nBytesRead - 4), "%2x %2x", &checkSumLsb, &checkSumMsb); advChecksum = checkSumLsb | (checkSumMsb << 8); if (checksum != advChecksum) { readError = True; Syslog::write("Doppler System WARNING : ensemble checksum incorrect\n" "Computed checksum = %d\n" "DVL checksum = %d\n", checksum, advChecksum); } if (readError) { m_output->data.badComms = 1; m_output->write(); return DeviceIF::Error; } else return DeviceIF::Ok; } // //////////////////////////////////////////////////////////////////////////////// // // Process the data that was just read: // //////////////////////////////////////////////////////////////////////////////// // virtual DeviceIF::Status Dvl::processRecord(unsigned char *record, int nRecordBytes) { Boolean debug = m_verbose; double velocity_RDI[NXE]; // // Initialize these each on each call. // bottomTrackVelocityStatus = 0; /* velocity status */ waterMassVelocityStatus = 0; dprintf("Dvl::processRecord() - record = %s \n", record); // // Parse the record: // sscanf( (char *) record+10, "%2x %2x %2x %2x %2x %2x %2x %2x", &xBottomTrackVelocity[DOPPLER_LSB], &xBottomTrackVelocity[DOPPLER_MSB], &yBottomTrackVelocity[DOPPLER_LSB], &yBottomTrackVelocity[DOPPLER_MSB], &zBottomTrackVelocity[DOPPLER_LSB], &zBottomTrackVelocity[DOPPLER_MSB], &eBottomTrackVelocity[DOPPLER_LSB], &eBottomTrackVelocity[DOPPLER_MSB]); sscanf( (char *) record+26, "%2x %2x %2x %2x %2x %2x %2x %2x", &rawRange[BM1][DOPPLER_LSB], &rawRange[BM1][DOPPLER_MSB], &rawRange[BM2][DOPPLER_LSB], &rawRange[BM2][DOPPLER_MSB], &rawRange[BM3][DOPPLER_LSB], &rawRange[BM3][DOPPLER_MSB], &rawRange[BM4][DOPPLER_LSB], &rawRange[BM4][DOPPLER_MSB]); sscanf( (char *) record+42, "%2x", bottomStatus); sscanf( (char *) record+44, "%2x %2x %2x %2x %2x %2x %2x %2x", &xWaterMassVelocity[DOPPLER_LSB], &xWaterMassVelocity[DOPPLER_MSB], &yWaterMassVelocity[DOPPLER_LSB], &yWaterMassVelocity[DOPPLER_MSB], &zWaterMassVelocity[DOPPLER_LSB], &zWaterMassVelocity[DOPPLER_MSB], &eWaterMassVelocity[DOPPLER_LSB], &eWaterMassVelocity[DOPPLER_MSB]); sscanf( (char *) record+68, "%2x", waterStatus); sscanf( (char *) record+70, "%2x %2x %2x %2x", &hour, &min, &sec, ¢isec); sscanf( (char *) record+86, "%2x %2x", &rawTemp[DOPPLER_LSB], &rawTemp[DOPPLER_MSB]); sscanf( (char *) record+96, "%2x %2x", &rawPitch[DOPPLER_LSB], &rawPitch[DOPPLER_MSB]); sscanf( (char *) record+100, "%2x %2x", &rawRoll[DOPPLER_LSB], &rawRoll[DOPPLER_MSB]); sscanf( (char *) record+104, "%2x %2x", &rawHeading[DOPPLER_LSB], &rawHeading[DOPPLER_MSB]); // // Convert the raw velocity reading to engineering units. // // The variable bottomTrackVelocityStatus essentially acts as a boolean that // is set if *bottomStatus != BEAM_OK or any of the velocity components is // equal to -32768. bottomStatus contains flags on the state of each beam. // waterMassVelocityStatus and waterStatus have analagous meanings for // the water-referenced velocity. // if (*bottomStatus == BEAM_OK) /* convert from mm/s to m/s */ { if (mmToMetres(xBottomTrackVelocity, yBottomTrackVelocity, zBottomTrackVelocity, eBottomTrackVelocity, velocity_RDI) == ERROR) { bottomTrackVelocityStatus = BAD_BOTTOM_TRACK_VELOCITY; } else /* mmToMetres successfully computed velocity_RDI */ { /* ** Convert from RDI ship coordinates (y ahead, z up) to SNAME ship ** ship coordinates (x ahead, z down). Beware that this transformation ** only operates on the first 3 elements of velocity_RDI. The error ** component, in the 4th element, is unchanged. */ TVMult( bottomTrackVelocity, T_SNAME_RDI, velocity_RDI ); bottomTrackVelocity[3] = velocity_RDI[3]; } } else /* beam error */ { if( m_verbose ) { if ((*bottomStatus & BOTTOM_BEAM1_CORRELATION) == BOTTOM_BEAM1_CORRELATION) Syslog::write("Doppler System WARNING : beam 1 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM1_ECHO_AMPLITUDE) == BOTTOM_BEAM1_ECHO_AMPLITUDE) Syslog::write("Doppler System WARNING : beam 1 low echo amplitude\n"); if ((*bottomStatus & BOTTOM_BEAM2_CORRELATION) == BOTTOM_BEAM2_CORRELATION) Syslog::write("Doppler System WARNING : beam 2 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM2_ECHO_AMPLITUDE) == BOTTOM_BEAM2_ECHO_AMPLITUDE) Syslog::write("Doppler System WARNING : beam 2 low echo amplitude\n"); if ((*bottomStatus & BOTTOM_BEAM3_CORRELATION) == BOTTOM_BEAM3_CORRELATION) Syslog::write("Doppler System WARNING : beam 3 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM3_ECHO_AMPLITUDE) == BOTTOM_BEAM3_ECHO_AMPLITUDE) Syslog::write("Doppler System WARNING : beam 3 low echo amplitude\n"); if ((*bottomStatus & BOTTOM_BEAM4_CORRELATION) == BOTTOM_BEAM4_CORRELATION) Syslog::write("Doppler System WARNING : beam 4 low correlation\n"); if ((*bottomStatus & BOTTOM_BEAM4_ECHO_AMPLITUDE) == BOTTOM_BEAM4_ECHO_AMPLITUDE) Syslog::write("Doppler System WARNING : beam 4 low echo amplitude\n"); } bottomTrackVelocityStatus = BAD_BOTTOM_TRACK_VELOCITY; } /* check the water-track velocity status */ if (*waterStatus == BEAM_OK) /* convert from mm/s to m/s */ { if (mmToMetres(xWaterMassVelocity, yWaterMassVelocity, zWaterMassVelocity, eWaterMassVelocity, velocity_RDI) == ERROR) { waterMassVelocityStatus = BAD_WATER_MASS_VELOCITY; } else /* mmToMetres successfully computed velocity_RDI */ { /* ** Convert from RDI ship coordinates (y ahead, z up) to SNAME ship ** ship coordinates (x ahead, z down). Beware that this transformation ** only operates on the first 3 elements of velocity_RDI. The error ** component, in the 4th element, is unchanged. */ TVMult( waterMassVelocity, T_SNAME_RDI, velocity_RDI ); waterMassVelocity[3] = velocity_RDI[3]; } } else /* beam error */ { if( m_verbose ) { if ((*waterStatus & ALTITUDE_TOO_SHALLOW) == ALTITUDE_TOO_SHALLOW) Syslog::write("Doppler System WARNING : altitude too shallow\n"); if ((*waterStatus & WATER_BEAM1_CORRELATION) == WATER_BEAM1_CORRELATION) Syslog::write("Doppler System WARNING : beam 1 low correlation\n"); if ((*waterStatus & WATER_BEAM2_CORRELATION) == WATER_BEAM2_CORRELATION) Syslog::write("Doppler System WARNING : beam 2 low correlation\n"); if ((*waterStatus & WATER_BEAM3_CORRELATION) == WATER_BEAM3_CORRELATION) Syslog::write("Doppler System WARNING : beam 3 low correlation\n"); if ((*waterStatus & WATER_BEAM4_CORRELATION) == WATER_BEAM4_CORRELATION) Syslog::write("Doppler System WARNING : beam 4 low correlation\n"); } waterMassVelocityStatus = BAD_WATER_MASS_VELOCITY; } *dvlDataStatus = (bottomTrackVelocityStatus | waterMassVelocityStatus); /* ** Extract the time of the ping, in seconds: */ *dvlPingTime = ( (double) hour )*3600. + ( (double) min )*60. + (double) sec + ( (double) centisec )/100.; dprintf(" dvlPingTime = %f\n", *dvlPingTime ); /* ** Convert the temperature into Degrees centigrade. */ *dvlTemp = (double) ( rawTemp[DOPPLER_LSB] | (rawTemp[DOPPLER_MSB] << 8) ); *dvlTemp /= 100.; /* 100 counts = 1 Degree C */ dprintf(" dvlTemp = %f\n", *dvlTemp ); /* ** Convert the tilt sensor pitch into Degrees */ *dvlPitch = (double) ( rawPitch[DOPPLER_LSB] | (rawPitch[DOPPLER_MSB] << 8) ); *dvlPitch /= 100.; /* 100 counts = 1 Degree */ dprintf(" dvlPitch = %f\n", *dvlPitch ); /* ** Convert the tilt sensor Roll into Degrees */ *dvlRoll = (double) ( rawRoll[DOPPLER_LSB] | (rawRoll[DOPPLER_MSB] << 8) ); *dvlRoll /= 100.; /* 100 counts = 1 Degree */ dprintf(" dvlRoll = %f\n", *dvlRoll ); /* ** Convert the DVL compass heading into Degrees */ *dvlHeading = (double) ( rawHeading[DOPPLER_LSB] | (rawHeading[DOPPLER_MSB] << 8) ); *dvlHeading /= 100.; /* 100 counts = 1 Degree */ dprintf(" dvlHeading = %f\n", *dvlHeading ); /* ** Compute range: */ status = cvtRange( rawRange, dvlRange ); if( status == ERROR ) { Syslog::write(" Doppler System - Error in range computation.\n"); } // // Write dvlDataStatus to the output array. It needs a cast, so it wasn't // done with a pointer. // #if !USE_POINTERS for( int i=0; idata.bottomTrackVelocity[i] = bottomTrackVelocity[i]; m_output->data.waterMassVelocity[i] = waterMassVelocity[i]; } m_output->data.bottomStatus = *bottomStatus; m_output->data.waterStatus = *waterStatus; m_output->data.pingTime = *dvlPingTime; m_output->data.temp = *dvlTemp; m_output->data.range = *dvlRange; m_output->data.pitch = *dvlPitch; m_output->data.roll = *dvlRoll; m_output->data.heading = *dvlHeading; #endif m_output->data.badComms = 0; //Comms are good if we got here. m_output->data.dataStatus = (long) *dvlDataStatus; m_output->write(); m_log->write(); return DeviceIF::Ok; } /******************************************************************************/ /* Function : cvtRange */ /* Purpose : Converts raw DVL range measurement into Eng. units of meters. */ /* Inputs : 4 beam range-to-bottoms, lsb and msb. */ /* Outputs : Returns OK or ERROR on bad velocity status. */ /******************************************************************************/ Boolean Dvl::cvtRange(long rawRange[NUM_BEAMS][NBYTES], double *dvlAltitude) { short rangeCM; /* range in centimeters */ double range = 0.; short i; for( i=0; i