7 #include <simgear/constants.h>
8 #include <simgear/io/sg_file.hxx>
9 #include <simgear/math/sg_geodesy.hxx>
10 #include <simgear/misc/sgstream.hxx>
11 #include <simgear/misc/strutils.hxx>
12 #include <simgear/misc/stdint.hxx>
20 #define START_OF_MSG0 147
21 #define START_OF_MSG1 224
24 UGEARTrack::UGEARTrack() {};
25 UGEARTrack::~UGEARTrack() {};
28 // swap the 1st 4 bytes with the last 4 bytes of a stargate double so
29 // it matches the PC representation
30 static double sg_swap_double( uint8_t *buf, size_t offset ) {
34 for ( size_t i = 0; i < 4; ++i ) {
35 tmpbuf[i] = buf[offset + i + 4];
37 for ( size_t i = 0; i < 4; ++i ) {
38 tmpbuf[i + 4] = buf[offset + i];
41 // for ( size_t i = 0; i < 8; ++i ) {
42 // printf("%d ", tmpbuf[i]);
46 result = (double *)tmpbuf;
51 static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
52 uint8_t cksum0, uint8_t cksum1 )
59 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
63 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
65 for ( uint8_t i = 0; i < size; i++ ) {
66 c0 += (uint8_t)buf[i];
68 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
69 // << " [" << (unsigned int)buf[i] << "]" << endl;
72 cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
73 << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
76 if ( c0 == cksum0 && c1 == cksum1 ) {
84 void UGEARTrack::parse_msg( const int id, char *buf,
85 struct gps *gpspacket, imu *imupacket,
86 nav *navpacket, servo *servopacket )
88 if ( id == GPS_PACKET ) {
89 *gpspacket = *(struct gps *)buf;
90 gpspacket->lat = sg_swap_double( (uint8_t *)buf, 0 );
91 gpspacket->lon = sg_swap_double( (uint8_t *)buf, 8 );
92 gpspacket->alt = sg_swap_double( (uint8_t *)buf, 16 );
93 gpspacket->vn = sg_swap_double( (uint8_t *)buf, 24 );
94 gpspacket->ve = sg_swap_double( (uint8_t *)buf, 32 );
95 gpspacket->vd = sg_swap_double( (uint8_t *)buf, 40 );
96 gpspacket->time = sg_swap_double( (uint8_t *)buf, 56 );
97 } else if ( id == IMU_PACKET ) {
98 *imupacket = *(struct imu *)buf;
99 imupacket->p = sg_swap_double( (uint8_t *)buf, 0 );
100 imupacket->q = sg_swap_double( (uint8_t *)buf, 8 );
101 imupacket->r = sg_swap_double( (uint8_t *)buf, 16 );
102 imupacket->ax = sg_swap_double( (uint8_t *)buf, 24 );
103 imupacket->ay = sg_swap_double( (uint8_t *)buf, 32 );
104 imupacket->az = sg_swap_double( (uint8_t *)buf, 40 );
105 imupacket->hx = sg_swap_double( (uint8_t *)buf, 48 );
106 imupacket->hy = sg_swap_double( (uint8_t *)buf, 56 );
107 imupacket->hz = sg_swap_double( (uint8_t *)buf, 64 );
108 imupacket->Ps = sg_swap_double( (uint8_t *)buf, 72 );
109 imupacket->Pt = sg_swap_double( (uint8_t *)buf, 80 );
110 imupacket->phi = sg_swap_double( (uint8_t *)buf, 88 );
111 imupacket->the = sg_swap_double( (uint8_t *)buf, 96 );
112 imupacket->psi = sg_swap_double( (uint8_t *)buf, 104 );
113 imupacket->time = sg_swap_double( (uint8_t *)buf, 116 );
114 // printf("imu.time = %.4f\n", imupacket->time);
115 } else if ( id == NAV_PACKET ) {
116 *navpacket = *(struct nav *)buf;
117 navpacket->lon = sg_swap_double( (uint8_t *)buf, 0 );
118 navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
119 navpacket->alt = sg_swap_double( (uint8_t *)buf, 16 );
120 navpacket->vn = sg_swap_double( (uint8_t *)buf, 24 );
121 navpacket->ve = sg_swap_double( (uint8_t *)buf, 32 );
122 navpacket->vd = sg_swap_double( (uint8_t *)buf, 40 );
123 navpacket->time = sg_swap_double( (uint8_t *)buf, 52 );
124 } else if ( id == SERVO_PACKET ) {
125 *servopacket = *(struct servo *)buf;
126 servopacket->time = sg_swap_double( (uint8_t *)buf, 20 );
127 // printf("servo time = %.3f\n", servopacket->time);
129 cout << "unknown id = " << id << endl;
134 // load the specified file, return the number of records loaded
135 bool UGEARTrack::load( const string &file ) {
146 double servo_time = 0;
154 SGFile input( file );
155 if ( !input.open( SG_IO_IN ) ) {
156 cout << "Cannot open file: " << file << endl;
160 while ( ! input.eof() ) {
161 // cout << "looking for next message ..." << endl;
162 int id = next_message( &input, NULL, &gpspacket, &imupacket,
163 &navpacket, &servopacket );
166 if ( id == GPS_PACKET ) {
167 if ( gpspacket.time > gps_time ) {
168 gps_data.push_back( gpspacket );
169 gps_time = gpspacket.time;
171 cout << "oops att back in time" << endl;
173 } else if ( id == IMU_PACKET ) {
174 if ( imupacket.time > imu_time ) {
175 imu_data.push_back( imupacket );
176 imu_time = imupacket.time;
178 cout << "oops pos back in time" << endl;
180 } else if ( id == NAV_PACKET ) {
181 if ( navpacket.time > nav_time ) {
182 nav_data.push_back( navpacket );
183 nav_time = navpacket.time;
185 cout << "oops pos back in time" << endl;
187 } else if ( id == SERVO_PACKET ) {
188 if ( servopacket.time > servo_time ) {
189 servo_data.push_back( servopacket );
190 servo_time = servopacket.time;
192 cout << "oops pos back in time" << endl;
197 cout << "processed " << count << " messages" << endl;
202 // attempt to work around some system dependent issues. Our read can
203 // return < data than we want.
204 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
208 result = ch->read( buf, length );
209 // cout << "wanted " << length << " read " << result << " bytes" << endl;
210 if ( ch->get_type() == sgFileType ) {
211 myeof = ((SGFile *)ch)->eof();
215 if ( result > 0 && log != NULL ) {
216 log->write( buf, result );
222 // attempt to work around some system dependent issues. Our read can
223 // return < data than we want.
224 int serial_read( SGSerialPort *serial, SGIOChannel *log,
225 char *buf, int length )
231 while ( bytes_read < length ) {
232 result = serial->read_port( tmp, length - bytes_read );
233 if ( result > 0 && log != NULL ) {
234 log->write( buf, result );
236 bytes_read += result;
238 // cout << " read " << bytes_read << " of " << length << endl;
244 // load the next message of a real time data stream
245 int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
246 gps *gpspacket, imu *imupacket, nav *navpacket,
252 // cout << "in next_message()" << endl;
256 // scan for sync characters
257 uint8_t sync0, sync1;
258 myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
259 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
260 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
262 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
263 // cout << "scanning for start of message "
264 // << (unsigned int)sync0 << " " << (unsigned int)sync1
265 // << ", eof = " << ch->eof() << endl;
266 if ( ch->get_type() == sgFileType ) {
267 myeof = ((SGFile *)ch)->eof();
271 cout << "found start of message ..." << endl;
273 // read message id and size
274 myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
275 myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
276 cout << "message = " << (int)id << " size = " << (int)size << endl;
279 if ( ch->get_type() == sgFileType ) {
280 int count = myread( ch, log, savebuf, size );
281 if ( count != size ) {
282 cout << "ERROR: didn't read enough bytes!" << endl;
285 #ifdef READ_ONE_BY_ONE
286 for ( int i = 0; i < size; ++i ) {
287 myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
290 myread( ch, log, savebuf, size );
295 myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
296 myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
298 if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
299 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
303 cout << "Check sum failure!" << endl;
308 // load the next message of a real time data stream
309 int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
310 gps *gpspacket, imu *imupacket, nav *navpacket,
317 // cout << "in next_message()" << endl;
321 // scan for sync characters
322 uint8_t sync0, sync1;
323 result = serial_read( serial, log, tmpbuf, 2 );
324 sync0 = (unsigned char)tmpbuf[0];
325 sync1 = (unsigned char)tmpbuf[1];
326 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
328 serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
329 cout << "scanning for start of message "
330 << (unsigned int)sync0 << " " << (unsigned int)sync1
334 // cout << "found start of message ..." << endl;
336 // read message id and size
337 serial_read( serial, log, tmpbuf, 2 );
338 uint8_t id = (unsigned char)tmpbuf[0];
339 uint8_t size = (unsigned char)tmpbuf[1];
340 cout << "message = " << (int)id << " size = " << (int)size << endl;
343 serial_read( serial, log, savebuf, size );
346 serial_read( serial, log, tmpbuf, 2 );
347 uint8_t cksum0 = (unsigned char)tmpbuf[0];
348 uint8_t cksum1 = (unsigned char)tmpbuf[1];
349 // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
352 if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
353 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
358 cout << "Check sum failure!" << endl;
365 static double interp( double a, double b, double p, bool rotational = false ) {
368 // special handling of rotational data
369 if ( diff > SGD_PI ) {
371 } else if ( diff < -SGD_PI ) {
379 gps UGEARInterpGPS( const gps A, const gps B, const double percent )
382 p.time = interp(A.time, B.time, percent);
383 p.lat = interp(A.lat, B.lat, percent);
384 p.lon = interp(A.lon, B.lon, percent);
385 p.alt = interp(A.alt, B.alt, percent);
386 p.ve = interp(A.ve, B.ve, percent);
387 p.vn = interp(A.vn, B.vn, percent);
388 p.vd = interp(A.vd, B.vd, percent);
389 p.ITOW = (int)interp(A.ITOW, B.ITOW, percent);
390 p.err_type = A.err_type;
395 imu UGEARInterpIMU( const imu A, const imu B, const double percent )
398 p.time = interp(A.time, B.time, percent);
399 p.p = interp(A.p, B.p, percent);
400 p.q = interp(A.q, B.q, percent);
401 p.r = interp(A.r, B.r, percent);
402 p.ax = interp(A.ax, B.ax, percent);
403 p.ay = interp(A.ay, B.ay, percent);
404 p.az = interp(A.az, B.az, percent);
405 p.hx = interp(A.hx, B.hx, percent);
406 p.hy = interp(A.hy, B.hy, percent);
407 p.hz = interp(A.hz, B.hz, percent);
408 p.Ps = interp(A.Ps, B.Ps, percent);
409 p.Pt = interp(A.Pt, B.Pt, percent);
410 p.phi = interp(A.phi, B.phi, percent);
411 p.the = interp(A.the, B.the, percent);
412 p.psi = interp(A.psi, B.psi, percent);
413 p.err_type = A.err_type;
418 nav UGEARInterpNAV( const nav A, const nav B, const double percent )
421 p.time = interp(A.time, B.time, percent);
422 p.lat = interp(A.lat, B.lat, percent);
423 p.lon = interp(A.lon, B.lon, percent);
424 p.alt = interp(A.alt, B.alt, percent);
425 p.ve = interp(A.ve, B.ve, percent);
426 p.vn = interp(A.vn, B.vn, percent);
427 p.vd = interp(A.vd, B.vd, percent);
428 p.err_type = A.err_type;
434 servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
437 for ( int i = 0; i < 8; ++i ) {
438 p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent);