Note: Goby version 1 (shown here) is now considered obsolete. Please use version 2 for new projects, and consider upgrading old projects.

Goby Underwater Autonomy Project  Series: 1.1, revision: 163, released on 2013-02-06 14:23:27 -0500
moos/pREMUSCodec/WhoiUtil.cpp
00001 #include <time.h>   // P.Brodsky
00002 #include <stdio.h>
00003 
00004 #include "WhoiUtil.h"
00005 
00006 LATLON_COMPRESSED Encode_latlon(double latlon_in) {
00007   /* Latitude and longitude are compressed into 3 byte values each. */ 
00008   LONG_AND_COMP out;
00009   double encoded;
00010   encoded = latlon_in * ((double)(0x007FFFFFL)/180.0);
00011   encoded += (encoded > 0.0) ? 0.5 : -0.5;
00012   // deal with truncation 
00013   out.as_long =(long int) encoded;
00014   return(out.as_compressed);
00015 } 
00016 
00017 
00018 double Decode_latlon(LATLON_COMPRESSED latlon_in) { 
00019   LONG_AND_COMP in; 
00020   in.as_long = 0;   //Clear the MSB
00021   in.as_compressed = latlon_in; 
00022   // this logic does not work for 64 bit architecture 
00023   //  if (in.as_long & 0x00800000L) // sign extend 
00024   //  in.as_long |= 0xFF000000L; 
00025   long all_f = -1; 
00026   if (in.as_long & 0x00800000L) // sign extend
00027     //  in.as_long |= 0xFFFFFFFFFF000000L; 
00028     in.as_long |= (all_f & !0xFFFFFL); 
00029 
00030   // P.Brodsky - wrap first (double) below
00031   //  return((double)in.as_long * (180.0/(double)(0x007FFFFFL)));
00032   // H. Schmidt version 
00033   double dval = (double)in.as_long * (180.0/(double)(0x007FFFFFL));
00034   if (dval > 180.0)
00035     dval -= 360.0; 
00036   return(dval);
00037 }
00038 
00039 unsigned char Encode_heading(float heading) 
00040 {
00041   return((unsigned char)(heading * 255.0/360.0 + 0.5));
00042 } 
00043 
00044 double Decode_heading(unsigned char heading) 
00045 {
00046   return((double)heading * 360.0 / 255.0 );
00047 } 
00048 
00049 /* Encodes velocity in meters per second into a single byte with a 
00050  * resolution of 2.5 cm/sec. Input range is 0- ~6 meters/second 
00051  * (12 knots). 
00052  */ 
00053 char Encode_est_velocity(float est_velocity) 
00054 {
00055   return((char)(est_velocity * 25.0));
00056 } 
00057 
00058 float Decode_est_velocity(char est_velocity) 
00059 {
00060   return(est_velocity/25.0);
00061 } 
00062 
00063 /* Code-decode salinity in range of 20-45 parts per thousand at a 
00064  * resolution of 0.1 part per thousand. */ 
00065 unsigned char Encode_salinity(float salinity) 
00066 {
00067   float output;
00068   if (salinity < 20.0) return(0);
00069   else 
00070     {
00071       output = (salinity - 20.0) * 10;
00072       if (output > 255) output = 255;
00073       return((unsigned char)output);
00074     }
00075 }
00076 
00077 float Decode_salinity(unsigned char sal) 
00078 {
00079   if (sal == 0) return(sal);
00080   return(((float)sal/10.0) + 20.0);
00081 }
00082 
00083 unsigned short Encode_depth(float depth) 
00084      /* 0 -100 meters: 0-1000 (10 cm resolution) 
00085       * 100 -200 meters: 1001-1500 (20 cm resolution) 
00086       * 200 -1000 meters: 1501-3100 (50 cm resolution) 
00087       * 1000-6000 meters: 3101-8100 (1 meter resolution) */ 
00088 {
00089   if (depth < 0) return(0);
00090   if (depth < 100) return((short unsigned int)((depth+.05) * 1.0/0.1));
00091   if (depth < 200) return((short unsigned int)(((depth-100) + 0.1 ) * 1.0/0.2 + 1000));
00092   if (depth < 1000) return((short unsigned int)(((depth-200) + 0.25) * 1.0/0.5 + 1500));
00093   if (depth < 6000) return((short unsigned int)(((depth-1000) + 0.5 ) * 1.0/1.0 + 3100));
00094   return(8100);
00095 }
00096 
00097 float Decode_depth(unsigned short depth) 
00098 {
00099   /* 0 -100 meters: 0-1000 (10 cm resolution) 
00100    * 100 -200 meters: 1001-1500 (20 cm resolution) 
00101    * 200 -1000 meters: 1501-3100 (50 cm resolution) 
00102    * 1000-6000 meters: 3101-8100 (1 meter resolution) 
00103    */ 
00104   unsigned short DEPTH_MODE_MASK = 0x1FFF;  // P.Brodsky
00105   depth &= DEPTH_MODE_MASK;  // Only using bottom 13 bits. 
00106   if (depth <= 1000) return(depth * 0.1/1.0);
00107   else if (depth <= 1500) return(100 + (depth-1000) * 0.2/1.0);
00108   else if (depth <= 3100) return(200 + (depth-1500) * 0.5/1.0);
00109   else if (depth <= 8100) return(1000 + (depth-3100) * 1.0/1.0);
00110   else return(6000);
00111 }
00112 
00113 unsigned char Encode_temperature(float temperature) 
00114 {
00115   if (temperature < -4) temperature = -4;
00116   temperature += 4;
00117   temperature = temperature * 256.0/40.0 + 0.5;
00118   if (temperature > 255) temperature = 255;
00119   return((unsigned char)temperature);
00120 }
00121 
00122 float Decode_temperature(unsigned char temperature) 
00123 { return(temperature * 40.0/256.0 - 4.0); }
00124 
00125 unsigned char Encode_sound_speed(float sound_speed) 
00126 { return((unsigned char)((sound_speed - 1425.0) * 2)); }
00127 
00128 float Decode_sound_speed(unsigned char sound_speed) 
00129 { return((float)sound_speed/2.0 + 1425.0); }
00130 
00131 unsigned short Encode_hires_altitude(float alt) /* 10 cm resolution to 655 meters. */ 
00132 {
00133   alt *= 100;
00134   if (alt > 65535.0)
00135     return(65535U);
00136   else if (alt < 0) return(0);
00137   else return((unsigned short)alt);
00138 }
00139 
00140 float Decode_hires_altitude(unsigned short alt) 
00141 { return((float)alt/100.0); }
00142 
00143 unsigned short Encode_gfi_pitch_oil(float gfi, float pitch, float oil) 
00144 {
00145   /* We are encoding 3 parameters that are somewhat specific to 
00146    * the REMUS 6000 into 2 bytes. * GFI= 5 bits: 0-100 (3.3 resolution) 
00147    * OIL= 5 bits: this gives us resolution of 3.3 percent 
00148    * Pitch=6 bits;
00149    180 into 64, resolution of 3 degrees. */ 
00150   unsigned short result;
00151   unsigned short temp;
00152   if (gfi < 0) // 5 bits of gfi 
00153     gfi = 0;
00154   else if (gfi > 100) gfi = 100;
00155   result = (unsigned short)(gfi * 31.0/100.0);
00156   if (oil < 0) // 5 bits of oil 
00157     oil = 0;
00158   else if (oil > 100) oil = 100;
00159   oil *= 31.0/100.0;
00160   result |= ((unsigned short)oil << 5);
00161   if (pitch > 90) // 6 bits of pitch 
00162     pitch = 90;
00163   else if (pitch < -90) pitch = -90;
00164   pitch *= 63.0/180.0;
00165   // Scale to 6 bits;
00166   if (pitch > 0.0) pitch += 0.5;
00167   else if (pitch < 0) pitch -= 0.5;
00168   temp = ((short)pitch << 10);
00169   result |= temp & 0xFC00;
00170   return(result);
00171 }
00172 
00173 void Decode_gfi_pitch_oil(unsigned short gfi_pitch_oil, 
00174                           float *gfi, float *pitch, float *oil) 
00175 {
00176   unsigned short temp;
00177   short temp_pitch;
00178   
00179   temp = (unsigned int)((gfi_pitch_oil & 0x001F) * 100.0/31.0);
00180   *gfi = temp;
00181   temp = (gfi_pitch_oil & 0x03E0) >> 5;
00182   *oil = temp * 100.0/31.0;
00183   temp_pitch = ((short)(gfi_pitch_oil & 0xFC00)) >> 10;
00184   *pitch = temp_pitch * 180.0/63.0;
00185 }
00186 
00187 TIME_DATE Encode_time_date(long secs_since_1970) 
00188 {
00189   /* The time is encoded as follows: 
00190    * Month 4 bits * Day 5 bits 
00191    * hour 5 bits 
00192    * Min 6 bits 
00193    * Secs 4 bits 
00194    // 4 secs per bit 
00195    * The year is not encoded. 
00196 */ 
00197   struct tm tm;
00198   TIME_DATE_LONG comp;
00199   /* Note: substitute localtime() for local timezone 
00200    * instead of GMT. */ 
00201   tm = *gmtime(&secs_since_1970);
00202   comp.as_long = (unsigned long)tm.tm_sec >> 2;
00203   comp.as_long += (unsigned long)tm.tm_min << 4;
00204   comp.as_long += (unsigned long)tm.tm_hour << (4 + 6);
00205   comp.as_long += (unsigned long)tm.tm_mday << (4 + 6 + 5);
00206   comp.as_long += (unsigned long)(tm.tm_mon+1) << (4 + 6 + 5 + 5);
00207   return(comp.as_time_date);
00208 }
00209 
00210 long Decode_time_date(TIME_DATE input, short *mon, short *day, short *hour, 
00211                       short *min, short *sec) 
00212 {
00213   TIME_DATE_LONG comp;
00214   comp.as_long = 0;
00215   comp.as_time_date = input;
00216   
00217   *mon = (short)((comp.as_long >> (4 + 6 + 5 + 5)) & 0x000F);
00218   *day = (short)((comp.as_long >> (4 + 6 + 5)) & 0x001F);
00219   *hour = (short)((comp.as_long >> (4 + 6)) & 0x001F);
00220   *min = (short)((comp.as_long >> (4)) & 0x003F);
00221   *sec = (short)(((comp.as_long ) & 0x000F) * 4);
00222 
00223   /* It is possible to force this routine to return the 
00224    * seconds since 1970. Left as an exercise for the reader… 
00225    */ 
00226   return (0);
00227 }
00228 
00229 unsigned char Encode_watts(float volts, float amps) 
00230 {
00231   /* Resolution is 4 watts, max is unsaturated is 1018 watts. */ 
00232   float watts = (volts * amps)/4;
00233   if (watts > 255) watts = 255;
00234   else if (watts < 0) watts = 0;
00235   // ok, not possible... 
00236   return((unsigned char)watts);
00237 }
00238 
00239 float Decode_watts(unsigned char watts_encoded) 
00240 {
00241   return((float)watts_encoded * 4.0);
00242 }
00243 
00244 char Encode_speed(SPEED_MODE mode, float speed) 
00245 {
00246   /* Max RPM: 2540 * Max Speed: 4.2 meters/sec (8.1 knots) */ 
00247   switch(mode) 
00248     {
00249     case SPEED_MODE_RPM: // Clamp to avoid errors. speed /= 20.0;
00250       if (speed > 127) speed = 127;
00251       else if (speed < -127) speed = -127;
00252       break;
00253     case SPEED_MODE_KNOTS: // Convert to m/sec speed *= 0.5144444;
00254       // Fall thru...
00255     case SPEED_MODE_MSEC: speed *= 30;
00256       if (speed > 127) speed = 127;
00257       else if (speed < -127) speed = -127;
00258       break;
00259     }
00260   return((char)speed);
00261 }
00262 
00263 float Decode_speed(SPEED_MODE mode, char speed) 
00264 {
00265   switch(mode) 
00266     {
00267     case SPEED_MODE_RPM: 
00268       return(speed * 20.0);
00269     case SPEED_MODE_MSEC: 
00270       return((float)speed/30.0);
00271     case SPEED_MODE_KNOTS: // "Knots mode not supported by modem codec" 
00272       return(0);
00273     }
00274   return(0);
00275 }
00276 
00277 double DecodeRangerLL(unsigned char c1, unsigned char c2,unsigned char c3, unsigned char c4,unsigned char c5){
00278  int deg100,deg10,deg1,min10,min1,minp1000,minp100,minp10,minp1,sign;
00279  double fMin,fDeg,fSign,fRet;
00280  
00281  // deg
00282  deg100 = ( (c1&0xf0) >> 4 ) ;
00283  deg10 = c1 & 0x0f ;
00284  deg1 = ( (c2&0xf0) >> 4 ) ;
00285  fDeg = deg100*100.0 + deg10*10.0 + deg1*1.0 ;
00286  
00287  // sign
00288  sign = c2 & 0x0f ;
00289  if ((sign==0x0c)||(sign==0x0d))
00290   fSign = -1.0 ;
00291  else 
00292   fSign = 1.0 ;
00293  
00294  // min
00295  min10 = ( (c3&0xf0) >> 4 ) ;
00296  min1 = c3 & 0x0f ;
00297  minp1000 = ( (c4&0xf0) >> 4 ) ;
00298  minp100 = c4 & 0x0f ;
00299  minp10 = ( (c5&0xf0) >> 4 ) ;
00300  minp1 = c5 & 0x0f ;
00301  fMin = min10*10.0 + min1*1.0 + minp1000*0.1 + minp100*0.01 + minp10*0.001 + minp1*0.0001 ;
00302  
00303  fRet = ( fDeg + (fMin / 60.0) ) * fSign ;
00304  
00305  return fRet;
00306 }
00307 
00308 double DecodeRangerBCD2(unsigned char c1, unsigned char c2){
00309  int i1000,i100,i10,i1;
00310  
00311  i1000 = ( (c1&0xf0) >> 4 ) ;
00312  i100 = c1 & 0x0f ;
00313  i10 = ( (c2&0xf0) >> 4 ) ;
00314  i1 = c2 & 0x0f ;
00315  
00316  return i1000*1000.0 + i100*100.0 + i10*10.0 + i1 *1.0 ;
00317 }
00318 
00319 double DecodeRangerBCD(unsigned char c1){
00320  int i10,i1;
00321  
00322  i10 = ( (c1&0xf0) >> 4 ) ;
00323  i1 = c1 & 0x0f ;
00324 
00325  return i10*10.0 + i1*1.0 ;
00326  
00327 }
00328 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends