/****************************************************************************/ /* Copyright (c) 2000 MIT Sea Grant */ /* MIT Sea Grant Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : Lbl.cc */ /* Author : Don Eickstedt using basic class provided by MBARI */ /* Project : */ /* Version : 1.0 */ /* Created : 05/25/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include "Lbl.h" #include "WorkSiteIF.h" #include "Syslog.h" #include "NavUtils.h" #include "AttributeParser.h" #include "FloatAttribute.h" #include "BooleanAttribute.h" #include "System.h" #include "Attributes.h" #include "TimeP.h" Lbl::Lbl(const char *name, NavSensors *sensors, int maxBad) : NavSensor(name, sensors, maxBad) { _lblIF = 0; _tofSampleTime = 0.; lbl_params.ping_time = 0.; //set response flag -- used for error checking on LBL response = True; } TaskInterface *Lbl::createTaskIF(int timeout) { TimeIF::TimeSpec ping_time; LblIF::RTimes rtimes; struct timespec timeSpec; double seconds; //this is vital to the proper functioning of the vehicle software sleep(3); //establish interface to Lbl Server try { _lblIF = new LblIF(name(), timeout); } catch (...) { _lblIF = 0; } //if interface to server established, load worksite parameters into LBL array // if( _lblIF != 0) Load_WorkSite(); else Syslog::write("Lbl NavSensor - could not create LBL task interface"); return _lblIF; // //Initialize _lastPingTime // _lblIF->get_ptime(&ping_time); //convert time format timeSpec.tv_sec = ping_time.seconds; timeSpec.tv_nsec = ping_time.nanoSeconds; _lastPingTime = Time::seconds(&timeSpec); Syslog::write("Lbl NavSensor - Initializing _lastPingTime to %f",_lastPingTime); // //Initialize _tofSampleTime _lblIF->get_tof(&rtimes); //convert time format timeSpec.tv_sec = rtimes.sampleTime.seconds; timeSpec.tv_nsec = rtimes.sampleTime.nanoSeconds; _tofSampleTime = Time::seconds(&timeSpec); Syslog::write("Lbl NavSensor - Initializing _tofSampleTime to %f", _tofSampleTime); } DeviceIF::Status Lbl::readTaskIF(Boolean *valid,TimeIF::TimeSpec *sampleTime) { long rval; LblIF::RTimes rtimes; TimeIF::TimeSpec ping_time; struct timespec timeSpec; float temp[10]; Boolean bad_hit; double seconds; Boolean debug = False; bad_hit = False; //Always reset ping time //ping time only non-zero on the cycle the ping occurs lbl_params.ping_time = 0.0; //check for new ping _lblIF->get_ptime(&ping_time); dprintf(" Lbl: _lblIF->get_ptime = %f\n", ping_time); //convert time format timeSpec.tv_sec = ping_time.seconds; timeSpec.tv_nsec = ping_time.nanoSeconds; seconds = Time::seconds(&timeSpec); dprintf(" Lbl: _lblIF->get_ptime = %f\n", seconds); if (seconds > _lastPingTime) { Syslog::write("Lbl::readTaskIF - Got new ping time. Interval = %f", seconds - _lastPingTime); // // Pass the nonzero ping time through to the lbl nav algo (jerome_position). // lbl_goats2000 assumes that a ping has been issued when // lbl_params.ping_time is nonzero. // lbl_params.ping_time = seconds; //check to see if previous ping had no reply if (response == False) bad_hit = True; //set response flag response = False; _lastPingTime = seconds; } //reset tofs each cycle for (int i = 1; i <= lbl_params.number_of_beacon; i++) lbl_params.tof[i] = 0.0; //check for new TOFs available _lblIF->get_tof(&rtimes); //convert time format timeSpec.tv_sec = rtimes.sampleTime.seconds; timeSpec.tv_nsec = rtimes.sampleTime.nanoSeconds; seconds = Time::seconds(&timeSpec); if (seconds > _tofSampleTime) { Syslog::write("Lbl::readTaskIF - Got new return times."); // The idl language won't let me pass arrays, so.... // I'll fix this in a later rev temp[0] = rtimes.t1; temp[1] = rtimes.t2; temp[2] = rtimes.t3; temp[3] = rtimes.t4; temp[4] = rtimes.t5; temp[5] = rtimes.t6; temp[6] = rtimes.t7; temp[7] = rtimes.t8; temp[8] = rtimes.t9; temp[9] = rtimes.t10; //map return times to individual beacons for (int i = 1; i <= lbl_params.number_of_beacon; i++) lbl_params.tof[i] = temp[_beacon_channel[i]-1]; //reset response flag response = True; _tofSampleTime = seconds; } //OK, nav algorithm checks for data *valid = True; if (bad_hit == False) return DeviceIF::Ok; else { // // I'm not sure that we should report an error here if it fails to hear // a single ping, since it defaults to dead-reckoning in the absence of // pings. We probably need some logic here. Also, the valid flag should // be set. // // *valid = False; return DeviceIF::Error; } } int Lbl::Load_WorkSite() { double easting,northing,depth; long turnaround,channel,result,_utmZone; char fullConfigFile[256]; Boolean verbose; double ping_lockout,range_gate; Attributes attributes("ps8000config"); WorkSiteIF workSite("workSite"); lbl_params.sos = workSite.soundSpeed(); lbl_params.number_of_beacon = (int) workSite.nLblTransponders(); for (short i = 1; i <= lbl_params.number_of_beacon; i++) { //read transponder information for each transponder if ((result = workSite.getTransponder(i-1,&easting,&northing,&depth,&turnaround,&channel)) == -1) Syslog::write ("Lbl Nav Sensor -- getTransponder failed"); else { lbl_params.eb [i] = easting; lbl_params.nb [i] = northing; lbl_params.db [i] = depth; lbl_params.turn_around_time[i] = (double) (turnaround/1000.0); //convert to seconds //this is used to map the return times to the appropriate beacon _beacon_channel [i] = (short) channel; } } _utmZone = workSite.utmZone(); //initialize ping time lbl_params.ping_time = 0.0; //convert initial position to UTM //initial pos should be in UTM in workSite.cfg to be consistent with beacon positions, will change later NavUtils::geoToUtm(workSite.latitude(), workSite.longitude(), _utmZone, &lbl_params.north_init, &lbl_params.east_init); //initialize TOFs for (i = 1; i <= lbl_params.number_of_beacon; i++) lbl_params.tof[i] = 0.0; //load configuration file to get timeout value strcpy(fullConfigFile, System::configurationFile("ps8000.cfg")); attributes.add( new FloatAttribute("ping_lockout", "Ping lockout (s)", &ping_lockout, 0.03 ) ); attributes.add( new FloatAttribute("range_gate", "Upper range gate", &range_gate, 1.75 ) ); attributes.add( new BooleanAttribute("verbose", "Verbosity", &verbose, True ) ); try { AttributeParser::parse(fullConfigFile, &attributes); } catch(...) { Syslog::write("PS8000 -- error loading configuration file"); } //set initial timeout value -- needed by jerome's nav software lbl_params.timeout = range_gate; lbl_params.cycle = 10.0; lbl_params.range_tol = 30.0; lbl_params.fix_err = 25.0; lbl_params.fixgain = 0.5; //0.2835 return 0; }