DCCL v3
WhoiUtil.cpp
1 // Copyright 2009-2017 Toby Schneider (http://gobysoft.org/index.wt/people/toby)
2 // GobySoft, LLC (for 2013-)
3 // Massachusetts Institute of Technology (for 2007-2014)
4 // Community contributors (see AUTHORS file)
5 //
6 //
7 // This file is part of the Dynamic Compact Control Language Library
8 // ("DCCL").
9 //
10 // DCCL is free software: you can redistribute it and/or modify
11 // it under the terms of the GNU Lesser General Public License as published by
12 // the Free Software Foundation, either version 2.1 of the License, or
13 // (at your option) any later version.
14 //
15 // DCCL is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 // GNU Lesser General Public License for more details.
19 //
20 // You should have received a copy of the GNU Lesser General Public License
21 // along with DCCL. If not, see <http://www.gnu.org/licenses/>.
22 #include <time.h> // P.Brodsky
23 #include <stdio.h>
24 #include <iostream>
25 #include <iomanip>
26 
27 #include "WhoiUtil.h"
28 
29 LATLON_COMPRESSED Encode_latlon(double latlon_in) {
30  /* Latitude and longitude are compressed into 3 byte values each. */
31  LONG_AND_COMP out;
32  double encoded;
33  encoded = latlon_in * ((double)(0x007FFFFFL)/180.0);
34  encoded += (encoded > 0.0) ? 0.5 : -0.5;
35  // deal with truncation
36  // std::cout << std::setprecision(16) << encoded << std::endl;
37  out.as_long =(long int) encoded;
38  // std::cout << std::hex << out.as_long << std::endl;
39  return(out.as_compressed);
40 }
41 
42 
43 double Decode_latlon(LATLON_COMPRESSED latlon_in) {
44  LONG_AND_COMP in;
45  in.as_long = 0; //Clear the MSB
46  in.as_compressed = latlon_in;
47 
48  if (in.as_long & 0x00800000L) // if is negative,
49  in.as_long |= ~0xFFFFFFL; // sign extend
50 
51  return (double)in.as_long * (180.0/(double)(0x007FFFFFL));
52 }
53 
54 unsigned char Encode_heading(float heading)
55 {
56  return((unsigned char)(heading * 255.0/360.0 + 0.5));
57 }
58 
59 double Decode_heading(unsigned char heading)
60 {
61  return((double)heading * 360.0 / 255.0 );
62 }
63 
64 /* Encodes velocity in meters per second into a single byte with a
65  * resolution of 2.5 cm/sec. Input range is 0- ~6 meters/second
66  * (12 knots).
67  */
68 char Encode_est_velocity(float est_velocity)
69 {
70  return((char)(est_velocity * 25.0 + 0.5)); // added 0.5 to perform basic positive rounding
71 }
72 
73 float Decode_est_velocity(char est_velocity)
74 {
75  return(est_velocity/25.0);
76 }
77 
78 /* Code-decode salinity in range of 20-45 parts per thousand at a
79  * resolution of 0.1 part per thousand. */
80 unsigned char Encode_salinity(float salinity)
81 {
82  float output;
83  if (salinity < 20.0) return(0);
84  else
85  {
86  output = (salinity - 20.0) * 10;
87  if (output > 255) output = 255;
88  return((unsigned char)output);
89  }
90 }
91 
92 float Decode_salinity(unsigned char sal)
93 {
94  if (sal == 0) return(sal);
95  return(((float)sal/10.0) + 20.0);
96 }
97 
98 unsigned short Encode_depth(float depth)
99  /* 0 -100 meters: 0-1000 (10 cm resolution)
100  * 100 -200 meters: 1001-1500 (20 cm resolution)
101  * 200 -1000 meters: 1501-3100 (50 cm resolution)
102  * 1000-6000 meters: 3101-8100 (1 meter resolution) */
103 {
104  if (depth < 0) return(0);
105  if (depth < 100) return((short unsigned int)((depth+.05) * 1.0/0.1));
106  if (depth < 200) return((short unsigned int)(((depth-100) + 0.1 ) * 1.0/0.2 + 1000));
107  if (depth < 1000) return((short unsigned int)(((depth-200) + 0.25) * 1.0/0.5 + 1500));
108  if (depth < 6000) return((short unsigned int)(((depth-1000) + 0.5 ) * 1.0/1.0 + 3100));
109  return(8100);
110 }
111 
112 float Decode_depth(unsigned short depth)
113 {
114  /* 0 -100 meters: 0-1000 (10 cm resolution)
115  * 100 -200 meters: 1001-1500 (20 cm resolution)
116  * 200 -1000 meters: 1501-3100 (50 cm resolution)
117  * 1000-6000 meters: 3101-8100 (1 meter resolution)
118  */
119  unsigned short DEPTH_MODE_MASK = 0x1FFF; // P.Brodsky
120  depth &= DEPTH_MODE_MASK; // Only using bottom 13 bits.
121  if (depth <= 1000) return(depth * 0.1/1.0);
122  else if (depth <= 1500) return(100 + (depth-1000) * 0.2/1.0);
123  else if (depth <= 3100) return(200 + (depth-1500) * 0.5/1.0);
124  else if (depth <= 8100) return(1000 + (depth-3100) * 1.0/1.0);
125  else return(6000);
126 }
127 
128 unsigned char Encode_temperature(float temperature)
129 {
130  if (temperature < -4) temperature = -4;
131  temperature += 4;
132  temperature = temperature * 256.0/40.0 + 0.5;
133  if (temperature > 255) temperature = 255;
134  return((unsigned char)temperature);
135 }
136 
137 float Decode_temperature(unsigned char temperature)
138 { return(temperature * 40.0/256.0 - 4.0); }
139 
140 unsigned char Encode_sound_speed(float sound_speed)
141 { return((unsigned char)((sound_speed - 1425.0) * 2)); }
142 
143 float Decode_sound_speed(unsigned char sound_speed)
144 { return((float)sound_speed/2.0 + 1425.0); }
145 
146 unsigned short Encode_hires_altitude(float alt) /* 10 cm resolution to 655 meters. */
147 {
148  alt *= 100;
149  if (alt > 65535.0)
150  return(65535U);
151  else if (alt < 0) return(0);
152  else return((unsigned short)alt);
153 }
154 
155 float Decode_hires_altitude(unsigned short alt)
156 { return((float)alt/100.0); }
157 
158 unsigned short Encode_gfi_pitch_oil(float gfi, float pitch, float oil)
159 {
160  /* We are encoding 3 parameters that are somewhat specific to
161  * the REMUS 6000 into 2 bytes. * GFI= 5 bits: 0-100 (3.3 resolution)
162  * OIL= 5 bits: this gives us resolution of 3.3 percent
163  * Pitch=6 bits;
164  180 into 64, resolution of 3 degrees. */
165  unsigned short result;
166  unsigned short temp;
167  if (gfi < 0) // 5 bits of gfi
168  gfi = 0;
169  else if (gfi > 100) gfi = 100;
170  result = (unsigned short)(gfi * 31.0/100.0);
171  if (oil < 0) // 5 bits of oil
172  oil = 0;
173  else if (oil > 100) oil = 100;
174  oil *= 31.0/100.0;
175  result |= ((unsigned short)oil << 5);
176  if (pitch > 90) // 6 bits of pitch
177  pitch = 90;
178  else if (pitch < -90) pitch = -90;
179  pitch *= 63.0/180.0;
180  // Scale to 6 bits;
181  if (pitch > 0.0) pitch += 0.5;
182  else if (pitch < 0) pitch -= 0.5;
183  temp = ((short)pitch << 10);
184  result |= temp & 0xFC00;
185  return(result);
186 }
187 
188 void Decode_gfi_pitch_oil(unsigned short gfi_pitch_oil,
189  float *gfi, float *pitch, float *oil)
190 {
191  unsigned short temp;
192  short temp_pitch;
193 
194  temp = (unsigned int)((gfi_pitch_oil & 0x001F) * 100.0/31.0);
195  *gfi = temp;
196  temp = (gfi_pitch_oil & 0x03E0) >> 5;
197  *oil = temp * 100.0/31.0;
198  temp_pitch = ((short)(gfi_pitch_oil & 0xFC00)) >> 10;
199  *pitch = temp_pitch * 180.0/63.0;
200 }
201 
202 TIME_DATE Encode_time_date(long secs_since_1970)
203 {
204  /* The time is encoded as follows:
205  * Month 4 bits * Day 5 bits
206  * hour 5 bits
207  * Min 6 bits
208  * Secs 4 bits
209  // 4 secs per bit
210  * The year is not encoded.
211 */
212  struct tm tm;
213  TIME_DATE_LONG comp;
214  /* Note: substitute localtime() for local timezone
215  * instead of GMT. */
216  tm = *gmtime(&secs_since_1970);
217  comp.as_long = (unsigned long)tm.tm_sec >> 2;
218  comp.as_long += (unsigned long)tm.tm_min << 4;
219  comp.as_long += (unsigned long)tm.tm_hour << (4 + 6);
220  comp.as_long += (unsigned long)tm.tm_mday << (4 + 6 + 5);
221  comp.as_long += (unsigned long)(tm.tm_mon+1) << (4 + 6 + 5 + 5);
222  return(comp.as_time_date);
223 }
224 
225 long Decode_time_date(TIME_DATE input, short *mon, short *day, short *hour,
226  short *min, short *sec)
227 {
228  TIME_DATE_LONG comp;
229  comp.as_long = 0;
230  comp.as_time_date = input;
231 
232  *mon = (short)((comp.as_long >> (4 + 6 + 5 + 5)) & 0x000F);
233  *day = (short)((comp.as_long >> (4 + 6 + 5)) & 0x001F);
234  *hour = (short)((comp.as_long >> (4 + 6)) & 0x001F);
235  *min = (short)((comp.as_long >> (4)) & 0x003F);
236  *sec = (short)(((comp.as_long ) & 0x000F) * 4);
237 
238  /* It is possible to force this routine to return the
239  * seconds since 1970. Left as an exercise for the reader…
240  */
241  return (0);
242 }
243 
244 unsigned char Encode_watts(float volts, float amps)
245 {
246  /* Resolution is 4 watts, max is unsaturated is 1018 watts. */
247  float watts = (volts * amps)/4;
248  if (watts > 255) watts = 255;
249  else if (watts < 0) watts = 0;
250  // ok, not possible...
251  return((unsigned char)watts);
252 }
253 
254 float Decode_watts(unsigned char watts_encoded)
255 {
256  return((float)watts_encoded * 4.0);
257 }
258 
259 char Encode_speed(SPEED_MODE mode, float speed)
260 {
261  /* Max RPM: 2540 * Max Speed: 4.2 meters/sec (8.1 knots) */
262  switch(mode)
263  {
264  case SPEED_MODE_RPM: // Clamp to avoid errors. speed /= 20.0;
265  if (speed > 127) speed = 127;
266  else if (speed < -127) speed = -127;
267  break;
268  case SPEED_MODE_KNOTS: // Convert to m/sec speed *= 0.5144444;
269  // Fall thru...
270  case SPEED_MODE_MSEC: speed *= 30;
271  if (speed > 127) speed = 127;
272  else if (speed < -127) speed = -127;
273  break;
274  }
275 
276  speed += (speed > 0.0) ? 0.5 : -0.5;
277 
278  return((char)speed);
279 }
280 
281 float Decode_speed(SPEED_MODE mode, char speed)
282 {
283  switch(mode)
284  {
285  case SPEED_MODE_RPM:
286  return(speed * 20.0);
287  case SPEED_MODE_MSEC:
288  return((float)speed/30.0);
289  case SPEED_MODE_KNOTS: // "Knots mode not supported by modem codec"
290  return(0);
291  }
292  return(0);
293 }
294 
295 double DecodeRangerLL(unsigned char c1, unsigned char c2,unsigned char c3, unsigned char c4,unsigned char c5){
296  int deg100,deg10,deg1,min10,min1,minp1000,minp100,minp10,minp1,sign;
297  double fMin,fDeg,fSign,fRet;
298 
299  // deg
300  deg100 = ( (c1&0xf0) >> 4 ) ;
301  deg10 = c1 & 0x0f ;
302  deg1 = ( (c2&0xf0) >> 4 ) ;
303  fDeg = deg100*100.0 + deg10*10.0 + deg1*1.0 ;
304 
305  // sign
306  sign = c2 & 0x0f ;
307  if ((sign==0x0c)||(sign==0x0d))
308  fSign = -1.0 ;
309  else
310  fSign = 1.0 ;
311 
312  // min
313  min10 = ( (c3&0xf0) >> 4 ) ;
314  min1 = c3 & 0x0f ;
315  minp1000 = ( (c4&0xf0) >> 4 ) ;
316  minp100 = c4 & 0x0f ;
317  minp10 = ( (c5&0xf0) >> 4 ) ;
318  minp1 = c5 & 0x0f ;
319  fMin = min10*10.0 + min1*1.0 + minp1000*0.1 + minp100*0.01 + minp10*0.001 + minp1*0.0001 ;
320 
321  fRet = ( fDeg + (fMin / 60.0) ) * fSign ;
322 
323  return fRet;
324 }
325 
326 double DecodeRangerBCD2(unsigned char c1, unsigned char c2){
327  int i1000,i100,i10,i1;
328 
329  i1000 = ( (c1&0xf0) >> 4 ) ;
330  i100 = c1 & 0x0f ;
331  i10 = ( (c2&0xf0) >> 4 ) ;
332  i1 = c2 & 0x0f ;
333 
334  return i1000*1000.0 + i100*100.0 + i10*10.0 + i1 *1.0 ;
335 }
336 
337 double DecodeRangerBCD(unsigned char c1){
338  int i10,i1;
339 
340  i10 = ( (c1&0xf0) >> 4 ) ;
341  i1 = c1 & 0x0f ;
342 
343  return i10*10.0 + i1*1.0 ;
344 
345 }
346