/****************************************************************************/ /* Copyright (c) 2002 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : Hydroscat.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 06/21/2002 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include "TimeP.h" #include #include #include #include #include #include "Hydroscat.h" #include "HydroscatOutput.h" #include "Syslog.h" #define SAMPLE_PERIOD 0.25 // seconds #define READ_TIMEOUT 500 // milliseconds #define MaxRecordBytes 1100 #define CHECKPOS 60 #define CALIBRATIONFILE "hs2Calibration.dat" Hydroscat::Hydroscat(SerialDevice *serialDevice, Boolean verbose) : _verbose(verbose), SerialDeviceDriver("Hydroscat", serialDevice, MaxRecordBytes, "\xa", READ_TIMEOUT, 3) { _log = new HydroscatLog(this, DataLog::BinaryFormat); _output = new HydroscatOutput(HydroscatOutputName, SharedData::ReadWrite); _output->data.deviceReady = True; _output->write(); System::copyToLogDir(System::configurationFile(CALIBRATIONFILE)); _cal = new HS2Cal(); _test_i = 0; } Hydroscat::~Hydroscat() { stop(); delete _log; delete _output; delete _cal; } DeviceIF::Status Hydroscat::initialize() { Boolean debug = _verbose; _device->flushReceiveBuf(); _device->clearPort(); _checksumErrorCnt = 0; // Reset the unit's date and time // if (setTime(time((long*)0)) != DeviceIF::Ok) { return DeviceIF::Error; } // Set the unit's time resolution to high in order to record // fractions of a second // char cmd[200]; char resp[500]; resp[0] = '\0'; sprintf(cmd, "TIMERES,H\n"); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { sleep(1); // Compare unit's response to expected response try { _device->readUntil(resp, sizeof(resp), "\xa", READ_TIMEOUT); } catch(...){}; dprintf("Hydroscat:TIMERES response:%s", resp); _device->flushReceiveBuf(); // Check for error response // if (resp[0] == '!' || resp[0] == '?') { Syslog::write("Hydroscat:bad response to TIMERES cmd: %s", resp); return DeviceIF::Error; } } // Tell the unit to open the shutter // sprintf(cmd, "OPEN,0\n"); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { sleep(1); // Compare unit's response to expected response resp[0] = '\0'; try { _device->readUntil(resp, sizeof(resp), "\xa", 2000); // give it 2 seconds } catch (Exception e) { Syslog::write("%s::init() - driver caught exception:%s", name(), e.msg); } dprintf("Hydroscat:OPEN response:%s", resp); _device->flushReceiveBuf(); // Check for error response // if (resp[0] == '!' || resp[0] == '?') { Syslog::write("Hydroscat:non-fatal response to OPEN, continuing..."); } } // Set up the unit to report and log data // sprintf(cmd, "LOG,%f,0,0,0,0,0,0,0,0\n", SAMPLE_PERIOD); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { sleep(1); // Compare unit's response to expected response // Error should be in first couple lines // dprintf("Hydroscat:LOG response:"); for (int i = 0; i < 5; i++) { resp[0] = '\0'; try { _device->readUntil(resp, sizeof(resp), "\xa", 1000); } catch(...){}; dprintf("%s", resp); // Check for error response // if (resp[0] == '!' || resp[0] == '?') { Syslog::write("Hydroscat:bad response to LOG cmd: %s", resp); return DeviceIF::Error; } } _device->flushReceiveBuf(); // in case there's any more } return start(); } //////////////////////////////////////////////////////////////////////////////// // // //////////////////////////////////////////////////////////////////////////////// // DeviceIF::Status Hydroscat::readRecord(unsigned char *record, int maxRecordBytes, const char *recordTerminator, unsigned readTimeout, int *nBytesRead) { Boolean debug = _verbose; Boolean readError = False; try { // // Read data record from device // *nBytesRead = getNextSocketRecord(record, maxRecordBytes, recordTerminator, readTimeout); } catch (SerialDevice::TimedOut) { readError = True; dprintf("%s::readRecord() - serial device timed out", name()); Syslog::write("%s::readRecord() - record so far: %s", name(), record); } catch (SerialDevice::BufferFull) { readError = True; Syslog::write("%s::readRecord() - serial device buffer full", name()); } catch (Exception e) { readError = True; Syslog::write("%s::readRecord() - driver caught exception:%s", name(), e.msg); throw; } dprintf("%s::readRecord(): %d bytes => %s", name(), *nBytesRead, record); if (!readError) { // // Compute the checksum for the record that we just read in: // LSByte of the unsigned sum of all the bytes preceding the // checksum not including the first byte. // unsigned char chrVal, checksum = 0; short advChecksum; for (short chr = 1; chr < CHECKPOS; chr++) { sscanf( (char *)( record + chr ), "%c", &chrVal ); checksum += chrVal; } // // Now, read in the checksum that the HS-2 computed, and compare with the // one that we computed here. // sscanf( (char *) (record + CHECKPOS), "%2x", &advChecksum); dprintf("%s::readRecord(): Computed checksum = %d, HS-2 checksum = %d", name(), checksum, advChecksum); if (checksum != advChecksum) { readError = True; // Print message only if the record is not a housekeeping record // if (strncmp((const char*)record, "*H", 2)) { Syslog::write("%s::readRecord() ensemble checksum incorrect", name()); dprintf("%s:Computed checksum = %d / HS-2 checksum = %d\n", name(), checksum, advChecksum); if (_checksumErrorCnt++ > 5) { _checksumErrorCnt = 0; stop(); sleep(1); start(); } } } } if (readError) { return DeviceIF::Error; } else { dprintf("Good read"); return DeviceIF::Ok; } } DeviceIF::Status Hydroscat::processRecord(unsigned char *record, int nBytes) { Boolean debug = _verbose; unsigned short usval; short sval1, sval2, sval3; // Make sure the record is the correct length // if (nBytes < CHECKPOS) { Syslog::write("Hydroscat processRecord(): record too short(%d)", nBytes); return DeviceIF::Error; } // Record is a character string representing hexadecimal data // // Read the timestamp from the record // if (2 != sscanf((char*)(record+2), "%8x%2x", &_seconds, &usval)) { Syslog::write("Hydroscat: Error reading timestamp from this record: %s", record); return DeviceIF::Error; } _hseconds = usval; // Read the Snorm values (only 3 are in use) // if (3 != sscanf((char*)(record+12), "%4x%4x%4x", &_snorm1,&_snorm2,&_snorm3)) { Syslog::write("Hydroscat: Error reading snorm vals from this record: %s", record); return DeviceIF::Error; } // Read the Gain/Status values (only 3 are in use) // if (3 != sscanf((char*)(record+44), "%1x%1x%1x", &_gainstatus1, &_gainstatus2, &_gainstatus3)) { Syslog::write("Hydroscat: Error reading status vals from this record: %s", record); return DeviceIF::Error; } // Read the depth, temp, and error values // if (3 != sscanf((char*)(record+52),"%4x%2x%2x", &_depthraw,&_tempraw,&_error)) { Syslog::write("Hydroscat: Error reading error stat from this record: %s", record); return DeviceIF::Error; } // Calculate temperature // _temp = (((float)_tempraw)/5.) - 10.; // Log the record data // _output->data.seconds = _seconds; _output->data.hseconds= _hseconds; _output->data.snorm1 = _snorm1; _output->data.gainstatus1 = _gainstatus1; _output->data.snorm2 = _snorm2; _output->data.gainstatus2 = _gainstatus2; _output->data.snorm3 = _snorm3; _output->data.gainstatus3 = _gainstatus3; _output->data.depthraw = _depthraw; _output->data.tempraw = _tempraw; _output->data.temp = _temp; _output->data.error = _error; // Use test_data to populate output record // //substitute_test_data(); _calOut = _cal->calibrate(_output->data); // // Placing _calOut back in _output->data seems a bit roundabout, but it // allows us to use Frederic's code without significant modification. // _output->data.calculated.ready = _calOut.ready; _output->data.calculated.bb470 = _calOut.bb470; _output->data.calculated.bb676 = _calOut.bb676; _output->data.calculated.fl676_uncorr = _calOut.fl676_uncorr; //_output->data.calculated.fl676_uncorr = 2.5e-3; //_calOut.fl676_uncorr = _output->data.calculated.fl676_uncorr; _output->write(); _log->write(); return DeviceIF::Ok; } //////////////////////////////////////////////////////////////////////////////////////////////// // Some test data and a test function for HS2Cal testing // #define TEST_DATA_VECTOR_SIZE 10 int test_snorm1[] = { 902, 1012, 1008, 1118, 1142, 1109, 1171, 1134, 1115, 1097}; int test_snorm2[] = {1288, 1389, 1336, 1329, 1438, 1329, 1272, 1287, 1305, 1276}; int test_snorm3[] = { 209, 240, 197, 221, 274, 275, 218, 242, 225, 285}; int test_gainstatus1[] = {5, 5, 5, 5, 5, 5, 5, 5, 5, 5}; int test_gainstatus2[] = {5, 5, 5, 5, 5, 5, 5, 5, 5, 5}; int test_gainstatus3[] = {5, 5, 5, 5, 5, 5, 5, 5, 5, 5}; int test_depthraw[] = {2380, 2391, 2401, 2405, 2403, 2412, 2414, 2420, 2433, 2440}; int test_tempraw[] = {122, 122, 123, 122, 122, 123, 123, 123, 122, 123}; double test_temp[] = {1440., 1440., 1460., 1440., 1440., 1460., 1460., 1460., 1440., 1460.}; int test_error[] = {32, 32, 32, 32, 32, 32, 32, 32, 32, 32}; void Hydroscat::substitute_test_data() { if (_test_i >= TEST_DATA_VECTOR_SIZE) _test_i = 0; else _test_i++; _output->data.snorm1 = test_snorm1[_test_i]; _output->data.gainstatus1 = test_gainstatus1[_test_i]; _output->data.snorm2 = test_snorm2[_test_i]; _output->data.gainstatus2 = test_gainstatus2[_test_i]; _output->data.snorm3 = test_snorm3[_test_i]; _output->data.gainstatus3 = test_gainstatus3[_test_i]; _output->data.depthraw = test_depthraw[_test_i]; _output->data.tempraw = test_tempraw[_test_i]; _output->data.temp = test_temp[_test_i]; _output->data.error = test_error[_test_i]; } //////////////////////////////////////////////////////////////////////////////// // Tell the HS-2 to stop processing // DeviceIF::Status Hydroscat::stop() { Syslog::write("Hydroscat:stopping Hydroscat..."); _device->flushReceiveBuf(); char cmd[100]; sprintf(cmd, "STOP\n"); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { delay(500); _device->flushReceiveBuf(); return DeviceIF::Ok; } else { Syslog::write("Hydroscat:stop failure"); return DeviceIF::Error; } } /////////////////////////////////////////////////////////////////////////////// // // This routine commands the unit to start processing using the stored // phase setup. // /////////////////////////////////////////////////////////////////////////////// // DeviceIF::Status Hydroscat::start() { Syslog::write("Hydroscat:starting Hydroscat..."); char cmd[100]; sprintf(cmd, "START,0\n"); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { Syslog::write("Hydroscat:started"); return DeviceIF::Ok; } else { Syslog::write("Hydroscat:start failure"); return DeviceIF::Error; } } //////////////////////////////////////////////////////////////////////////////// // // This routine commands the unit to reset it's internal clock to a given // time. The input is the number of seconds since 1970/01/01 00:00:00. // //////////////////////////////////////////////////////////////////////////////// // DeviceIF::Status Hydroscat::setTime(unsigned long t1970) { Boolean debug = _verbose; // Use the DATE command to reset the unit's date/time to match the MVC's // struct tm *hytime = localtime((time_t*)&t1970); char cmd[100]; char resp[500]; sprintf(cmd, "DATE,%02d/%02d/%4d %02d:%02d:%02d\xa", hytime->tm_mon+1, hytime->tm_mday, hytime->tm_year+1900, hytime->tm_hour, hytime->tm_min, hytime->tm_sec); if (_device->write(cmd, strlen(cmd)) == strlen(cmd)) { sleep(1); try { _device->readUntil(resp, sizeof(resp), "\xa", 1000); } catch (Exception e) { Syslog::write("%s::setTime() - driver caught exception:%s", name(), e.msg); } dprintf("Hydroscat:setTime() - response:%s", resp); _device->flushReceiveBuf(); // Check for error response // if (resp[0] == '!' || resp[0] == '?') { Syslog::write("Hydroscat:bad response to DATE cmd: %s", resp); return DeviceIF::Error; } } return DeviceIF::Ok; } //////////////////////////////////////////////////////////////////////////////// // // This routine returns a 'T' data record or will timeout doing so // //////////////////////////////////////////////////////////////////////////////// // int Hydroscat::getNextRecord(unsigned char *record, int maxRecordBytes, const char *recordTerminator, unsigned readTimeout) { Boolean debug = _verbose; int nBytesRead = 0; try { // Read and return a data record from device. // Discard housekeeping records // First echo, then discard info records // Boolean dataRead = False; while (!dataRead) { nBytesRead = _device->readUntil((char *)record, maxRecordBytes, (char *)recordTerminator, readTimeout); if (nBytesRead > 1) { // Data records start with "*T" // if (record[0] == '*' && record[1] == 'T') dataRead = True; // Info records start with a quote (') or a bang (!) // else if (record[0] == '!' && record[1] == '\'') Syslog::write("Hydroscat info: %s", record); else if (record[0] == '*' && record[1] == 'H') dprintf("Hydroscat housekeeping info: %s", record); else dprintf("Unhandled Hydroscat record: %s", record); } } } catch (Exception e) { throw; } return nBytesRead; } int Hydroscat::getNextSocketRecord(unsigned char *record, int maxRecordBytes, const char *recordTerminator, unsigned readTimeout) { Boolean debug = False; Boolean readError = False; Boolean noRecord = True; unsigned char* eor = NULL; int nBytes = 0; int i; long t0, t1; t0 = t1 = Time::milliseconds(); while (noRecord) { // Convert from milliseconds to timeval format // Data was recv()ed from the device // nBytes += recv(_device->getFd(), (char*)(record+nBytes), (maxRecordBytes - nBytes - 1), MSG_PEEK); // Look for end-of-record marker in the buffer // record[nBytes] = '\0'; dprintf("Seabird/%s::readRecord() - recv()ed %d bytes %s\n", name(), nBytes, record); for (i = 0; i < nBytes; i++) { if (record[i] == *recordTerminator) { eor = record+i; nBytes = i+1; break; } } if (eor != (char *)NULL) { // Got a complete record noRecord = False; readError = False; read(_device->getFd(), (char *)(record), nBytes); long readTime = Time::milliseconds() - t0; } else if (nBytes == (maxRecordBytes-1)) { // Not a record yet, make sure we didn't reach the end // of the buffer. If we have, then nothing to do except // start recv()ing at the beginning of the buffer again. // Syslog::write("SerialDevice::Buffer full!"); readError = True; nBytes = 0; ioctl(_device->getFd(), TIOCFLUSH, 0); } // Time is up, break out of the while loop t1 = Time::milliseconds(); if (noRecord && (t0 + readTimeout) <= t1) { readError = True; break; } } return nBytes; }