/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : Parosci.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include #include #include #include #include #include "Parosci.h" #include "Syslog.h" #include "AttributeParser.h" #include "IntegerAttribute.h" #include "WorkSiteIF.h" #include "SerialDevice.h" #define PARO_MODE_ATTEMPTS 3 static short parosciPeriod = 10; static char *INT_RESET_CONFIRM_STRING ="*0001PR=00024"; static char *UNIT_CONFIRM_STRING ="0100UN\xd\xa"; static char *UNIT_CONFIRM_RESPONSE_STRING ="*0001UN="; static char *UNIT_RESET_STRING ="0100EW*0100UN=1\xd\xa"; static char *UNIT_RESET_RESPONSE ="*0001UN=1"; static char *FIXED_LENGTH_OUTPUT = "0100EW*0100DL=1\xd\xa"; #define REPLY_STR_LEN (17) static char *MODE_CONFIRM_STRING ="0100MD\xd\xa"; static char *MODE_CONFIRM_RESPONSE ="*0001MD=0"; static char *SANITY_CHECK_STRING ="0100P3\xd\xa"; static char *GET_PRESSURE_STRING ="0100P1\xd\xa"; static char *READ_PRESSURE_STRING ="*0100DB\xd\xa"; static char *STOP_PRESSURE_STRING = "*0100P1\xd\xa"; static char *GET_TEMP_PERIOD ="0100Q1\xd\xa"; static char *prefix ="0100"; typedef enum{ TC, C1, C2, C3, D1, D2, T1, T2, T3, T4, T5, U0, Y1, Y2, Y3 } parocoeff; static const char *Parosci::coeffNameList[] = { "TC", "C1", "C2", "C3", "D1", "D2","T1","T2","T3","T4","T5", "U0","Y1","Y2","Y3" }; static const double Parosci::kPsiToBar = 0.0689476; void timeoutHandler(int signo) { } //== The members ================================================= Parosci::Parosci(SerialDevice *device, int baro_corr_flag) : Task("Parosci"),_attributes("Parosci") { _device = device; _output = new ParosciOutput(SharedData::ReadWrite); _output->data.deviceStatus = DeviceIF::Initializing; _output->write(); _baro_corr_flag = baro_corr_flag; initialize(); } Parosci::~Parosci() { delete _output; delete _log; if (timer_id > 0) timer_delete(timer_id); } #define REPLY_LEN 32 void Parosci::startupPressurePeriod() { writeCommand(GET_PRESSURE_STRING); } DeviceIF::Status Parosci::initialize() { char command[100]; int val,i,serialNo; double pres_0; Boolean debug = True; // == Initialize class variables ================================ site_latitude = 0.0; pressure=0.0; depth=0.0; pres_period = -1.0; temp_period=-1.0; // == Configure the site latitude =============================== try { WorkSiteIF worksite("worksite"); site_latitude = worksite.latitude(); start_depth = worksite.getStartDepth(); } catch(SharedObjectClient::MissingServer e) { site_latitude = 45.0; Syslog::write("Parosci.cc -- Worksite server is not running. " "Using default lat of %lf", site_latitude ); } catch(...) { Syslog::write("Parosci.cc failed on instantiation of WorkSiteIF"); } // == Read pressure resolution and temperature interval from config file // _attributes.add(new IntegerAttribute("PR", "Pressure Resolution", &_pr, INT_TIME)); _attributes.add(new IntegerAttribute("TempInterval", "Take a temp period after this many pressure periods", &_tempInt, 10)); const char *configFileName = System::configurationFile("parosci.cfg"); AttributeParser::parse(configFileName, &_attributes); dprintf("Parosci.cc -- parsed PR (%d) and TempInterval (%d)", _pr, _tempInt); // Create a data file to record data about the instrument // const char* pdatfile = System::configurationFile("parosci.dat"); ofstream pdat(pdatfile, ios::trunc); long now; time(&now); pdat << "# " << ctime(&now) << "#" << endl; pdat << "# Paroscientific Data File" << endl; pdat << "# ========================" << endl; // == Configure the ParosciOutput =============================== _output->data.depth = 0.00; _output->data.pressure = 0.00; _output->data.temp = 0.00; _output->data.updateTime.tv_sec = 0.0; _output->data.deviceStatus = DeviceIF::Ok; try { _output->write(); } catch (SharedData::AccessError error) { fprintf(stderr, "%s",error.msg); exit(1); } // == Configure the periodic callback =========================== //addPeriodicCallback( parosciPeriod, // (CallbackMethod)Parosci::periodicParosciCallback ); //== Set up logger ============================================== _log = new ParosciLog( this, DataLog::BinaryFormat ); //== Initialize the serial port ================================= _device->commsDebugMode(SerialDevice::DebugOff); //_device->commsDebugMode(SerialDevice::DebugOn); _device->setLineFormat(BAUD, DATA_BITS, STOP_BITS, PARITY); _device->nonRaw(); _device->clearPort(); _device->flushReceiveBuf(); //== Get serial no =============================================== // writeCommand("0100SN\xd\xa"); if (_device->readNChars(reply, REPLY_LEN, PARO_TIMEOUT) != -1) // if (_device->read(reply, REPLY_LEN, PARO_TIMEOUT) != -1) { if (sscanf(reply, "%*7s=%d",&serialNo) !=1) //error { Syslog::write("Parosci.cc -- Failed to read serial number"); throw Exception("Failed to read serial numbre"); } } else { Syslog::write("Parosci.cc -- Failed to read serial number"); throw Exception("Failed to read serial numbre"); } dprintf("Parosci.cc -- Serial # is %d", serialNo); pdat << "# Serial Number = " << serialNo << endl; pdat << "#" << endl; pdat << "# Setup Information" << endl; pdat << "# -----------------" << endl; //== Put Parosci into proper mode =================================== // Try more than once - this has failed fairly regularly in // the past. // Record the mode in the data file pdat << "#\tMode: MD = 0" << endl; // Setting MD=0 int mi; for (mi = 0; mi < PARO_MODE_ATTEMPTS; mi++) { writeCommand("0100EW*0100MD=0\xd\xa"); if( _device->confirm("*0001MD=0\xd",PARO_TIMEOUT) == -1) { Syslog::write("Parosci.cc -- Failed to set mode"); continue; // error try again } else { // If it works once, break out of loop // dprintf("Parosci::init() - mode set okay"); break; } } if (mi == 3) { //error throw Exception("Parosci.cc -- Failed to set mode"); } // If we failed every time, there really is a problem // if (mi >= PARO_MODE_ATTEMPTS) { //error throw Exception("Parosci.cc -- Failed to set mode"); } //== Reduce integration times ================================== // // First, find out what the current setting is // writeCommand("0100PR\xd\xa"); if (_device->confirm("*0001PR=",PARO_TIMEOUT) == -1) { //error Syslog::write("Parosci.cc -- Failed to read integration times"); throw Exception("Failed to read integration times"); } if (_device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT) != -1) { if ( sscanf(reply,"%d",&val) !=1) { //error Syslog::write("Parosci.cc -- Failed to read integration times"); throw Exception("Failed to read integration times"); } dprintf("Parosci.cc -- PR reply = %d\n", val); // If the current setting differs from that in the config file, // change it // if (val != _pr) { Syslog::write("Parosci.cc -- Current integration time = %d", val ); Syslog::write("Parosci.cc -- setting to new time = %d", _pr); //reset integration time sprintf(command, "0100EW*0100PR=%d\xd\xa", _pr); writeCommand(command); delay(50); // Make sure device got PR command // //#if 0 sprintf(command, "*0001PR="); dprintf("Parosci.cc -- Confirming %s", command); if (_device->confirm(command, PARO_TIMEOUT) == -1) { //error Syslog::write("Parosci.cc -- Failed to set new integration times"); throw Exception("Failed to set new integration times"); } //#endif } } // Record the pressure resolution in the data file pdat << "#\tPressure Resolution: PR = " << _pr << endl; pdat << "#\tTemp Period every " << _tempInt << " Pressure Periods" << endl; //== Set the units on comp. pressure are in PSI ================== // writeCommand(UNIT_CONFIRM_STRING); if (_device->confirm(UNIT_CONFIRM_RESPONSE_STRING, PARO_TIMEOUT) == -1) { //error Syslog::write("Parosci.cc -- Failed to confirm units"); throw Exception("Failed to confirm units"); } // if (_device->read(reply, REPLY_LEN, PARO_TIMEOUT) != -1) if (_device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT) != -1) { if (sscanf(reply, "%d",&val) !=1) { Syslog::write("Parosci.cc -- Failed to confirm units"); throw Exception("Failed to confirm units"); } dprintf("Parosci.cc -- units = %d", val); // Set units to PSI if not that already // if (val != UNITS_VAL) { Syslog::write("Parosci.cc -- Current output units = %d", val ); Syslog::write("Parosci.cc -- Setting new units = %d", UNITS_VAL ); //reset units writeCommand(UNIT_RESET_STRING); if (_device->confirm(UNIT_RESET_RESPONSE, PARO_TIMEOUT) == -1) { //error Syslog::write("Parosci.cc -- Failed to set new units"); throw Exception("Failed to set new units"); } } } // Record the units code in the data file pdat << "#\tPressure Units (PSI): UN = " << UNITS_VAL << endl; // == Confirm MODE is 0 ==================================== // writeCommand(MODE_CONFIRM_STRING); if (_device->confirm(MODE_CONFIRM_RESPONSE, PARO_TIMEOUT) == -1) { //error Syslog::write("Parosci.cc -- Failed to confirm mode"); throw Exception("Failed to confirm mode"); } // == Retrieve conversion coefficients ====================== // delay(500); _device->clearPort(); // strcpy(msg_buf,"\t") pdat << "#" << endl; pdat << "# Device Coefficients" << endl; pdat << "# -------------------" << endl; for (i=0 ; i < sizeof(coeffNameList)/sizeof(char *) ; i++) { sprintf(command, "%s%s\xd\xa",prefix,coeffNameList[i]); writeCommand(command); delay(10); // get the reply // try { if (_device->readUntil(reply, REPLY_LEN, "\xa", 5000) == -1 || sscanf(reply,"%*7s=%lf",&pc[i]) !=1) { //error Syslog::write("Parosci.cc -- Failed to read coefficient %d (%d): %02x", i, strlen(reply), reply[0] ); } } catch(...) { Syslog::write("Parosci.cc -- could not get coefficient"); } pdat << "#\t" << coeffNameList[i] << " = " << pc[i] << endl; } delay(100); // == Test the setup ====================================== // // sanity check: ask instrument for compensated pressure, then get // raw pres & temp periods and calculate compensated pressure // using above coeffs. Then restart the pressure readings writeCommand(SANITY_CHECK_STRING); if (_device->confirm("*0001",PARO_TIMEOUT) != -1 && _device->read(reply, REPLY_LEN, PARO_TIMEOUT) > 0 && sscanf(reply, "%lf",&pres_0) == 1) { Syslog::write("Parosci.cc -- Initial pressure = %lf", psiToBar(pres_0) ); } else { //error Syslog::write("Parosci.cc -- Failed to query pressure for " "sanity check: %s",reply); throw Exception("Failed to query pressure for sanity check"); } // now get pressure period // startupPressurePeriod(); _device->confirm("*0001", 2*PARO_TIMEOUT); _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT); if (sscanf(reply, "%lf", &pres_period) == 1) { Syslog::write("Parosci.cc -- Parosci initial pressure period = %lf", pres_period ); } else { //error Syslog::write("Parosci.cc -- Failed to query pressure period" " for sanity check"); throw Exception("Failed to query pressure period for sanity check"); } //_device->write(STOP_PRESSURE_STRING, strlen(STOP_PRESSURE_STRING)); //tcdrain(_device->getFd()); delay(10); //_device->clearPort(); // and temperature period (this stops the pressure readouts) // writeCommand(GET_TEMP_PERIOD); // delay(10); if (_device->confirm("*0001",2*PARO_TIMEOUT) != -1 && _device->readUntil(reply, REPLY_LEN, "\xa", PARO_TIMEOUT) > 0 // && _device->read(reply, REPLY_LEN, PARO_TIMEOUT) > 0 && sscanf(reply,"%lf",&temp_period) == 1) { Syslog::write("Parosci.cc -- Parosci initial temp period = %lf", temp_period ); } else { //error Syslog::write("Parosci.cc -- Failed to query temperature period for " "sanity check"); throw Exception("Failed to query temperature period for sanity" " check"); } // startupPressurePeriod(); // Calculating initial pressure // init_temp = comp_temp( temp_period ); if (_baro_corr_flag) { init_pres = comp_press_bar(pres_period,temp_period); } else { init_pres = DEFAULT_BARO_CORRECTION_BAR; } writeCommand(FIXED_LENGTH_OUTPUT); // Hurray! Paroscientific initialization completed // Syslog::write("Parosci.cc -- Paroscientific configured."); _output->data.deviceStatus = DeviceIF::Ok; _output->write(); System::copyToLogDir(pdatfile); remove (pdatfile); // struct sigevent event; // event.sigev_signo = SIGUSR1; // timer_id = timer_create(CLOCK_REALTIME, &event); // signal(SIGUSR1, timeoutHandler); _device->raw(); _device->blocking(); return DeviceIF::Ok; } DeviceIF::Status Parosci::processRecord() { try { if( getParosci() == OK) { // Syslog::write("Parosci.cc -- pressure = %lf; temp = %lf; " // "depth = %lf", pressure, _output->data.temp, depth ); _output->data.pressure = pressure; _output->data.depth = depth; _output->data.surface_pressure = init_pres; _output->data.updateTime.tv_sec = readtime.tv_sec; _output->data.updateTime.tv_nsec = readtime.tv_nsec; _output->data.deviceStatus = DeviceIF::Ok; // -- Log the data ----------------------------------------- _log->write(); // -- and now make data available to server ---------------- _output->write(); return DeviceIF::Ok; } else { //changed to return OK even if there was a read error - hjt _output->data.deviceStatus = DeviceIF::Error; _output->write(); return DeviceIF::Error; } } catch(...) { Syslog::write("caught getParosci exception"); _output->data.deviceStatus = DeviceIF::Error; return DeviceIF::Error; } return DeviceIF::Ok; } // Write a command string to the Parosci in two parts. // First, write a '*', to stop the stream from the device. // Then write the command string. The command string should // NOT have a leading '*' // void Parosci::writeCommand(const char *cmd) { // Write the leading '*' required of every Parosci command // _device->clearPort(); _device->write("*", 1); _device->write(cmd, strlen(cmd)); } /////////////////////////////////////////////////////////////////////// // Add start_depth to depth calculation for docking missions // int Parosci::getParosci(void) { Boolean debug = False; int nconv; double value; static int parosci_count = 0; //number of times this routine is called static int temp_cycle = 10; static int last_read_was_temp = 0; //read temp every 10 cycles struct itimerspec timer; static int temp_reject_count=0, pres_reject_count=0; char c; int i; int nbytes; char buf[20]; pres_period = 0; temp_period = 0; _device->clearPort(); memset(reply, 0, REPLY_LEN); //read last requested pressure _device->write("*0100P1\xd\xa",9); // writeCommand(GET_PRESSURE_STRING); nbytes = ::dev_read(_device->getFd(), reply, REPLY_STR_LEN, REPLY_STR_LEN, 0, 50, 0, 0); if (nbytes != REPLY_STR_LEN) { Syslog::write("Parosci: Error - read of pressure string returned %d\n", nbytes); return ERROR; } reply[nbytes] = 0x0; //put null terminator in string if (strncmp("*0001", reply, 5) != 0) { Syslog::write("Parosci: Error - could not parse pressure header %s(%d)\n", reply, nbytes); return ERROR; } dprintf("Parosci.cc -- pressure reply with value is %s", reply); if (sscanf(reply+5,"%lf",&pres_period) != 1) { //read error Syslog::write("Parosci.cc -- Failure to parse reply: %s", reply); return ERROR; } _device->write("*0100Q1\xd\xa", 9); // writeCommand(GET_TEMP_PERIOD); nbytes = ::dev_read(_device->getFd(), reply, REPLY_STR_LEN, REPLY_STR_LEN, 0, 50, 0, 0); reply[nbytes] = 0x0; //put null terminator in string if (nbytes != REPLY_STR_LEN) { Syslog::write("Parosci: Error - read of temp string returned %d\n", nbytes); return ERROR; } if (strncmp("*0001", reply, 5) != 0) { Syslog::write("Parosci: Error - could not parse temp header %d(%s)\n", nbytes, reply); return ERROR; } dprintf("Parosci.cc -- temp reply with value is %s", reply); if (sscanf(reply+5,"%lf",&temp_period) != 1) { //read error Syslog::write("Parosci.cc -- Failure to parse reply: %s", reply); return ERROR; } dprintf("temp period is %f, pressure period is %f\n", temp_period, pres_period); _output->data.temp = comp_temp( temp_period ); pressure = comp_press_bar(pres_period, temp_period); if (pressure MIN_PRESSURE) { depth = corrected_depth (pressure - init_pres); depth += start_depth; // vehicle mission may start below the surface } else { //error bad pressure Syslog::write("Parosci.cc -- Error:Bad pressure %f bar\n", pressure); Syslog::write("Parosci - Error: %f pres period %f temp period\n", pres_period, temp_period); Syslog::write("Reply is %s\n", reply); return ERROR; } clock_gettime(CLOCK_REALTIME,&readtime); dprintf("Parosci.cc -- %d bytes buffered now", _device->nRecvdBytes()); return OK; } void Parosci::periodicParosciCallback( void ) { } void Parosci::run() { while (1) processRecord(); } //COMP_TEMP: calculates the temperature on the Parosci in degC // using the algorithm described in appendix G of the Parosci //Digiquartz Manual // double Parosci::comp_temp(double t_per) { double U, Temp; U = t_per - pc[U0]; Temp = (pc[Y1] + (pc[Y2] + pc[Y3]*U)*U)*U; // printf("Given t_per = %lf, U = %lf, temp = %lf\n", // t_per, U, Temp ); return Temp; } //== comp_press_bar ============================================= // comp_press_bar: calculates the temparature compensated pressure in bar // using the method in Appendix G of the Parosci Digiquartz manual // double Parosci::comp_press_bar(double p_per, double t_per) { double U, Temp; double Tn, C, D, T0, psi; U = t_per - pc[U0]; C = pc[C1] + (pc[C2] + pc[C3]*U)*U; D = pc[D1] + pc[D2]*U; T0 = pc[T1] + (pc[T2] + (pc[T3] + (pc[T4] + pc[T5]*U)*U)*U)*U; Tn = T0*T0/(p_per*p_per); psi = C*(1-Tn) * (1-D*(1-Tn)); return psiToBar(psi); } double Parosci::corrected_depth(double bar) { double corrected, dbar, C, gr; static double st2; static int first=1; if (first) // calc & store sin^2(latitude) { st2 = sin ( site_latitude); st2 *= st2; first = 0; } //convert to dbar dbar = 10*bar; C = (((-1.8200e-15*dbar + 2.279e-10)*dbar - 2.5212e-05)*dbar + 9.72659) * dbar; gr = 9.780318*((2.3600e-05*st2 + 5.2788e-3)*st2 + 1) + 1.092e-06*dbar; corrected = C/gr; return corrected; }