/****************************************************************************/ /* Copyright (c) 2000 MIT Sea Grant */ /* MIT Sea Grant Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : SimulatedLbl.cc */ /* Author : Don Eickstedt */ /* Project : */ /* Version : 1.0 */ /* Created : 07/15/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include #include #include "SimulatedLbl.h" #include "Syslog.h" #include "WorkSiteIF.h" #include "System.h" #include "AttributeParser.h" #include "FloatAttribute.h" #include "BooleanAttribute.h" SimLbl::SimLbl(const char *name):LblIF_SK() { char out[120]; int status; double easting,northing,depth; long turnaround,channel,result; char fullConfigFile[256]; Attributes attributes( "ps8000constants" ); Boolean verbose; _valid_times = False; _valid_ptime = False; _ping_pending = False; _log = new LblLog(); try{ _simulator = new SimulatorIF("simulator"); } catch(...){ throw("SimPS8000 could not establish interface to simulator server, aborting"); } WorkSiteIF workSite("workSite"); lbl.num_beacons = workSite.nLblTransponders(); lbl.sos = workSite.soundSpeed(); for (int i = 1; i <= lbl.num_beacons; 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.nb[i] = northing; lbl.eb[i] = easting; lbl.db[i] = depth; sprintf(out,"beacon %d - x = %f y = %f z = %f",i,lbl.nb[i],lbl.eb[i],lbl.db[i]); Syslog::write(out); //load beacon turnaround and convert to seconds lbl.turnaround[i] = ((double) turnaround)/1000.0; //this is used to set the timeouts for each of the ten channels _beacon_channel [i] = (short) channel; } } 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"); } } SimLbl::~SimLbl() { delete _log; delete _simulator; } //called by NavSensor every nav cycle //to check for return times from a previous ping long SimLbl::get_tof(LblIF::RTimes *tof) { //check to see if return times are back //if there has been a previous ping if (_ping_pending) check_for_times(); // // Always return the last set of tof's and the time they were recorded. // *tof = r_times; if (_valid_times) { //reset flag _valid_times = False; return 0L; } //no tofs available return -1L; } //called by Navsensor every nav cycle //to check for a new ping long SimLbl::get_ptime(TimeIF::TimeSpec *ping_time) { // // Always return the time of the last ping. // *ping_time = rp_time; if (_valid_ptime) { //reset flag _valid_ptime = False; return 0L; } //no ping time available return -1L; } //called by PS8000 driver when TOFs come back void SimLbl::times(LblIF::RTimes *rt) { //legacy interface support } //called by PS8000 driver when ping has occurred void SimLbl::ptime(TimeIF::TimeSpec *p_time) { //legacy interface support } //spawn driver process int SimLbl::spawnAuxTasks() { return 0; } // //send ping command to PS8000 driver // long SimLbl::ping() { SimulatorIF::Vector vel_B_N_B, omega_B_N_B,euler,current_position; double dist,time_out; char mess[200]; long result; int i; struct timespec p_time; Boolean debug = False; dprintf("SimLbl::ping() - pinging"); //get ping time clock_gettime(CLOCK_REALTIME,&p_time); ping_time = p_time.tv_sec + ((double) p_time.tv_nsec)/1000000000L; //save ping time rp_time.seconds = p_time.tv_sec; rp_time.nanoSeconds = p_time.tv_nsec; _valid_ptime = True; //write ping time to log time_out = rp_time.seconds + ((double) rp_time.nanoSeconds)/1000000000L; sprintf(mess,"p: %10.8f",time_out); _log->write(mess); //get current vehicle position result = _simulator->state(current_position,vel_B_N_B,euler,omega_B_N_B); //calculate one-way TOFs including beacon turnaround for (i = 1; i <= lbl.num_beacons; i++) { dist=dist3(lbl.nb[i],lbl.eb[i],lbl.db[i], current_position[0],current_position[1],current_position[2]); dprintf("SimLbl::ping() - "); dprintf("beacon %d pos (UTM) is: x = %f, y = %f, z = %f", i,lbl.nb[i],lbl.eb[i],lbl.db[i]); ow_tof[i] = (dist/lbl.sos)+lbl.turnaround[i]; dprintf("dist to beacon %d (m) = %f",i,dist); dprintf("one-way tof (s) to beacon %d = %f",i,ow_tof[i]); } //initialize return times and found flag for (i = 1;i <= lbl.num_beacons; i++) { found[i] = 0; returns[i] = 0.0; } //initialize last position and time last_position[0] = current_position[0]; last_position[1] = current_position[1]; last_position[2] = current_position[2]; last_time = ping_time; //initialize number of tofs found num_found = 0; //set flag _ping_pending = True; return 0L; } void SimLbl::check_for_times() { double dist[MAX_BEACONS],sdist[MAX_BEACONS],returns[MAX_BEACONS], s_lower,s_upper,s_int; SimulatorIF::Vector euler,current_position,vel_B_N_B, omega_B_N_B,interp_pos; double curr_time,rand_val,elapsed,speed,time_int,x_rate,y_rate,z_rate, s_time,s_elapsed; struct timespec now; long result; char out[200]; Boolean done; Boolean debug = False; //get current vehicle state result = _simulator->waterSpeed(&speed); result = _simulator->state(current_position,vel_B_N_B,euler,omega_B_N_B); //dprintf("vehicle state:, x = %f, y = %f, z = %f, speed = %f", // current_position[0],current_position[1],current_position[2],speed); //get current time clock_gettime(CLOCK_REALTIME,&now); //calculate elapsed time since ping curr_time = now.tv_sec + ((double) now.tv_nsec)/1000000000L; elapsed = curr_time - ping_time; //calculate current distance from auv to each beacon for (int i = 1;i <= lbl.num_beacons; i++) { dist[i]=dist3(lbl.nb[i],lbl.eb[i],lbl.db[i], current_position[0],current_position[1],current_position[2]); dprintf("distance to beacon %d = %f",i,dist[i]); //if not already arrived, check for arrival if ((found[i] != 1) && ((ow_tof[i] + (dist[i]/lbl.sos)) <= elapsed)) { //response for beacon i arrived within latest 200 msec period found[i] = 1; num_found++; //how long since last position? time_int = curr_time - last_time; //calculate vehicle velocity x_rate = (current_position[0]-last_position[0])/time_int; y_rate = (current_position[1]-last_position[1])/time_int; z_rate = (current_position[2]-last_position[2])/time_int; done = False; //set upper and lower bounds on current search interval s_upper = curr_time; s_lower = last_time; //since we know arrival time within 200 msec window, use a //binary search to find arrival time window to within some //arbitrary resolution while (!done) { //cut the interval in half s_time = s_lower + (s_upper-s_lower)/2.0; s_elapsed = s_time - ping_time; s_int = (s_time-last_time); //extrapolate vehicle position from velocity and //elapsed time from last known position interp_pos[0] = last_position[0] + (x_rate*s_int); interp_pos[1] = last_position[1] + (y_rate*s_int); interp_pos[2] = last_position[2] + (z_rate*s_int); sdist[i]=dist3(lbl.nb[i],lbl.eb[i],lbl.db[i], interp_pos[0],interp_pos[1],interp_pos[2]); //have we found return interval? if ((ow_tof[i] + (sdist[i]/lbl.sos)) < s_elapsed) { //is interval small enough? if ((s_time-s_lower) <= RESOLUTION) { done = True; //make sure we're within our allowed time gates if ((s_elapsed > ping_lockout) && (s_elapsed <= range_gate)) //shift residual error to be +/- RESOLUTION/2 //instead of 0..RESOLUTION returns[i] = (s_lower + (s_time-s_lower)/2.0)-ping_time; /*--- add noise and spurious measurements */ #ifdef NOISY rand_val = uniform_noise(); /*--- the tof value is set to be anywhere between 0 and its true value (=> erroneous tof) */ if (rand_val < 0.1) returns[i] = uniform_noise()*returns[i]; else /*--- the tof is affected by the largest noise (=> not bad, but not too good tof) */ if ((rand_val >= 0.1) && (rand_val < 0.25)) returns[i] = fabs(returns[i]+ gaussian_noise(0.0,TOF_NOT_SO_GOOD)); /*--- the tof is affected by the regular noise (=> good tof) */ else returns[i] = fabs(returns[i]+ gaussian_noise(0.0,TOF_STD)); #endif } else //shorten time interval s_upper = s_time; } else //solution lies in upper interval s_lower = s_time; } } } dprintf(" SimLbl::check_for_times - num_found = %d\n", num_found); dprintf(" SimLbl::check_for_times - num_beacons = %d\n", lbl.num_beacons); dprintf(" SimLbl::check_for_times - elapsed time = %f\n", elapsed); dprintf(" SimLbl::check_for_times - range_gate = %f\n", range_gate); if ((num_found == lbl.num_beacons) || (elapsed >= range_gate)) { //copy return times for (i = 1; i <= lbl.num_beacons; i++) switch (_beacon_channel[i]) { case 1: r_times.t1 = returns[i]; break; case 2: r_times.t2 = returns[i]; break; case 3: r_times.t3 = returns[i]; break; case 4: r_times.t4 = returns[i]; break; case 5: r_times.t5 = returns[i]; break; case 6: r_times.t6 = returns[i]; break; case 7: r_times.t7 = returns[i]; break; case 8: r_times.t8 = returns[i]; break; case 9: r_times.t9 = returns[i]; break; case 10: r_times.t10 = returns[i]; break; default: Syslog::write("PS8000 Simulator -- Illegal beacon channel\n"); } //Set the time at which the tofs (returns) are valid: r_times.sampleTime.seconds = now.tv_sec; r_times.sampleTime.nanoSeconds = now.tv_nsec; //make tofs available _valid_times = True; _ping_pending = False; //log values sprintf(out,"%2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f", r_times.t1,r_times.t2, r_times.t3,r_times.t4, r_times.t5,r_times.t6, r_times.t7,r_times.t8, r_times.t9,r_times.t10); //log values dprintf(" SimLbl::check_for_times() - \n"); dprintf("%2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f %2.5f\n", r_times.t1,r_times.t2, r_times.t3,r_times.t4, r_times.t5,r_times.t6, r_times.t7,r_times.t8, r_times.t9,r_times.t10); _log->write(out); dprintf("elapsed time = %f",elapsed); } else { //update for next 200 msec interval last_position[0] = current_position[0]; last_position[1] = current_position[1]; last_position[2] = current_position[2]; last_time = curr_time; } } //change range gates on PS8000 //not implemented yet long SimLbl::change_gate() { //not implemented yet return 0L; } LblLog::LblLog() { char *auvLogDir = getenv(AuvLogDirName); sprintf(_fileName, "%s/%s/%s.log", auvLogDir, LatestLogDirName,"SimLBL"); } LblLog::~LblLog() { } LblLog::write(char *message) { FILE *log; time_t time_of_day; char out_mess[120]; struct tm tmbuf; if ((log = fopen(_fileName,"a+"))==NULL) Syslog::write("Failure opening SimLBL Log"); else { time_of_day = time(NULL); _gmtime(&time_of_day,&tmbuf); strftime(out_mess,120,"%D %T ",&tmbuf); strcat(out_mess,message); fprintf(log,"%s\n",out_mess); fclose(log); } } double dist3(double x1,double y1,double z1,double x2,double y2,double z2) { return (sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1))); } double uniform_noise() { return ((double) rand()/RAND_MAX); } double gaussian_noise(double m,double std) { double fac,u1,u2,v1,v2,s; int u; static int iset = 0; static double gset; if (iset == 0) { s = 0; do { u = rand(); u1 = (double) u/RAND_MAX; u = rand(); u2 = (double) u/RAND_MAX; v1 = 2*u1-1; v2 = 2*u2-1; s = v1*v1+v2*v2; } while (s > 1); fac = sqrt ((-2 * log(s))/s); gset = (v1*fac); iset = 1; v2 = v2*fac*std+m; return v2; } else { iset = 0; return (gset*std) + m; } }