7 #include <simgear/constants.h>
8 #include <simgear/io/sg_file.hxx>
9 #include <simgear/math/sg_geodesy.hxx>
10 #include <simgear/misc/sg_path.hxx>
11 #include <simgear/misc/sgstream.hxx>
12 #include <simgear/misc/strutils.hxx>
13 #include <simgear/misc/stdint.hxx>
21 #define START_OF_MSG0 147
22 #define START_OF_MSG1 224
25 UGEARTrack::UGEARTrack():
30 UGEARTrack::~UGEARTrack() {};
33 // swap the 1st 4 bytes with the last 4 bytes of a stargate double so
34 // it matches the PC representation
35 static double sg_swap_double( uint8_t *buf, size_t offset ) {
39 for ( size_t i = 0; i < 4; ++i ) {
40 tmpbuf[i] = buf[offset + i + 4];
42 for ( size_t i = 0; i < 4; ++i ) {
43 tmpbuf[i + 4] = buf[offset + i];
46 // for ( size_t i = 0; i < 8; ++i ) {
47 // printf("%d ", tmpbuf[i]);
51 result = (double *)tmpbuf;
56 static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
57 uint8_t cksum0, uint8_t cksum1,
58 bool ignore_checksum )
60 if ( ignore_checksum ) {
69 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
73 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
75 for ( uint8_t i = 0; i < size; i++ ) {
76 c0 += (uint8_t)buf[i];
78 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
79 // << " [" << (unsigned int)(uint8_t)buf[i] << "]" << endl;
82 // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
83 // << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
86 if ( c0 == cksum0 && c1 == cksum1 ) {
94 void UGEARTrack::parse_msg( const int id, char *buf,
95 struct gps *gpspacket, imu *imupacket,
96 nav *navpacket, servo *servopacket,
97 health *healthpacket )
99 if ( id == GPS_PACKET ) {
100 *gpspacket = *(struct gps *)buf;
102 gpspacket->time = sg_swap_double( (uint8_t *)buf, 0 );
103 gpspacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
104 gpspacket->lon = sg_swap_double( (uint8_t *)buf, 16 );
105 gpspacket->alt = sg_swap_double( (uint8_t *)buf, 24 );
106 gpspacket->vn = sg_swap_double( (uint8_t *)buf, 32 );
107 gpspacket->ve = sg_swap_double( (uint8_t *)buf, 40 );
108 gpspacket->vd = sg_swap_double( (uint8_t *)buf, 48 );
110 } else if ( id == IMU_PACKET ) {
111 *imupacket = *(struct imu *)buf;
113 imupacket->time = sg_swap_double( (uint8_t *)buf, 0 );
114 imupacket->p = sg_swap_double( (uint8_t *)buf, 8 );
115 imupacket->q = sg_swap_double( (uint8_t *)buf, 16 );
116 imupacket->r = sg_swap_double( (uint8_t *)buf, 24 );
117 imupacket->ax = sg_swap_double( (uint8_t *)buf, 32 );
118 imupacket->ay = sg_swap_double( (uint8_t *)buf, 40 );
119 imupacket->az = sg_swap_double( (uint8_t *)buf, 48 );
120 imupacket->hx = sg_swap_double( (uint8_t *)buf, 56 );
121 imupacket->hy = sg_swap_double( (uint8_t *)buf, 64 );
122 imupacket->hz = sg_swap_double( (uint8_t *)buf, 72 );
123 imupacket->Ps = sg_swap_double( (uint8_t *)buf, 80 );
124 imupacket->Pt = sg_swap_double( (uint8_t *)buf, 88 );
125 imupacket->phi = sg_swap_double( (uint8_t *)buf, 96 );
126 imupacket->the = sg_swap_double( (uint8_t *)buf, 104 );
127 imupacket->psi = sg_swap_double( (uint8_t *)buf, 112 );
129 // printf("imu.time = %.4f size = %d\n", imupacket->time, sizeof(struct imu));
130 } else if ( id == NAV_PACKET ) {
131 *navpacket = *(struct nav *)buf;
133 navpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
134 navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
135 navpacket->lon = sg_swap_double( (uint8_t *)buf, 16 );
136 navpacket->alt = sg_swap_double( (uint8_t *)buf, 24 );
137 navpacket->vn = sg_swap_double( (uint8_t *)buf, 32 );
138 navpacket->ve = sg_swap_double( (uint8_t *)buf, 40 );
139 navpacket->vd = sg_swap_double( (uint8_t *)buf, 48 );
141 } else if ( id == SERVO_PACKET ) {
142 *servopacket = *(struct servo *)buf;
144 servopacket->time = sg_swap_double( (uint8_t *)buf, 0 );
146 // printf("servo time = %.3f\n", servopacket->time);
147 } else if ( id == HEALTH_PACKET ) {
148 *healthpacket = *(struct health *)buf;
150 healthpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
153 cout << "unknown id = " << id << endl;
158 // load the named stream log file into internal buffers
159 bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) {
171 double servo_time = 0;
172 double health_time = 0;
181 SGFile input( file );
182 if ( !input.open( SG_IO_IN ) ) {
183 cout << "Cannot open file: " << file << endl;
187 while ( ! input.eof() ) {
188 // cout << "looking for next message ..." << endl;
189 int id = next_message( &input, NULL, &gpspacket, &imupacket,
190 &navpacket, &servopacket, &healthpacket,
194 if ( id == GPS_PACKET ) {
195 if ( gpspacket.time > gps_time ) {
196 gps_data.push_back( gpspacket );
197 gps_time = gpspacket.time;
199 cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
201 } else if ( id == IMU_PACKET ) {
202 if ( imupacket.time > imu_time ) {
203 imu_data.push_back( imupacket );
204 imu_time = imupacket.time;
206 cout << "oops imu back in time" << endl;
208 } else if ( id == NAV_PACKET ) {
209 if ( navpacket.time > nav_time ) {
210 nav_data.push_back( navpacket );
211 nav_time = navpacket.time;
213 cout << "oops nav back in time" << endl;
215 } else if ( id == SERVO_PACKET ) {
216 if ( servopacket.time > servo_time ) {
217 servo_data.push_back( servopacket );
218 servo_time = servopacket.time;
220 cout << "oops servo back in time" << endl;
222 } else if ( id == HEALTH_PACKET ) {
223 if ( healthpacket.time > health_time ) {
224 health_data.push_back( healthpacket );
225 health_time = healthpacket.time;
227 cout << "oops health back in time" << endl;
232 cout << "processed " << count << " messages" << endl;
237 // load the named stream log file into internal buffers
238 bool UGEARTrack::load_flight( const string &path ) {
254 gzFile fservo = NULL;
255 gzFile fhealth = NULL;
261 file = path; file.append( "gps.dat.gz" );
262 if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) {
263 printf("Cannont open %s\n", file.c_str());
267 size = sizeof( struct gps );
268 printf("gps size = %d\n", size);
269 while ( gzread( fgps, &gpspacket, size ) == size ) {
270 gps_data.push_back( gpspacket );
274 file = path; file.append( "imu.dat.gz" );
275 if ( (fimu = gzopen( file.c_str(), "r" )) == NULL ) {
276 printf("Cannot open %s\n", file.c_str());
280 size = sizeof( struct imu );
281 printf("imu size = %d\n", size);
282 while ( gzread( fimu, &imupacket, size ) == size ) {
283 imu_data.push_back( imupacket );
287 file = path; file.append( "nav.dat.gz" );
288 if ( (fnav = gzopen( file.c_str(), "r" )) == NULL ) {
289 printf("Cannot open %s\n", file.c_str());
293 size = sizeof( struct nav );
294 printf("nav size = %d\n", size);
295 while ( gzread( fnav, &navpacket, size ) == size ) {
296 // printf("%.4f %.4f\n", navpacket.lat, navpacket.lon);
297 nav_data.push_back( navpacket );
300 // open the servo file
301 file = path; file.append( "servo.dat.gz" );
302 if ( (fservo = gzopen( file.c_str(), "r" )) == NULL ) {
303 printf("Cannot open %s\n", file.c_str());
307 size = sizeof( struct servo );
308 printf("servo size = %d\n", size);
309 while ( gzread( fservo, &servopacket, size ) == size ) {
310 servo_data.push_back( servopacket );
313 // open the health file
314 file = path; file.append( "health.dat.gz" );
315 if ( (fhealth = gzopen( file.c_str(), "r" )) == NULL ) {
316 printf("Cannot open %s\n", file.c_str());
320 size = sizeof( struct health );
321 printf("health size = %d\n", size);
322 while ( gzread( fhealth, &healthpacket, size ) == size ) {
323 health_data.push_back( healthpacket );
330 // attempt to work around some system dependent issues. Our read can
331 // return < data than we want.
332 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
336 result = ch->read( buf, length );
337 // cout << "wanted " << length << " read " << result << " bytes" << endl;
338 if ( ch->get_type() == sgFileType ) {
339 myeof = ((SGFile *)ch)->eof();
343 if ( result > 0 && log != NULL ) {
344 log->write( buf, result );
350 // attempt to work around some system dependent issues. Our read can
351 // return < data than we want.
352 int serial_read( SGSerialPort *serial, SGIOChannel *log,
353 char *buf, int length )
359 while ( bytes_read < length ) {
360 result = serial->read_port( tmp, length - bytes_read );
361 bytes_read += result;
363 // cout << " read " << bytes_read << " of " << length << endl;
366 if ( bytes_read > 0 && log != NULL ) {
367 log->write( buf, bytes_read );
374 // load the next message of a real time data stream
375 int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
376 gps *gpspacket, imu *imupacket, nav *navpacket,
377 servo *servopacket, health *healthpacket,
378 bool ignore_checksum )
383 // cout << "in next_message()" << endl;
387 // scan for sync characters
388 uint8_t sync0, sync1;
389 myread( ch, log, tmpbuf, 2 );
390 sync0 = (unsigned char)tmpbuf[0];
391 sync1 = (unsigned char)tmpbuf[1];
392 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
394 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
395 cout << "scanning for start of message "
396 << (unsigned int)sync0 << " " << (unsigned int)sync1
397 << ", eof = " << ch->eof() << endl;
398 if ( ch->get_type() == sgFileType ) {
399 myeof = ((SGFile *)ch)->eof();
403 cout << "found start of message ..." << endl;
405 // read message id and size
406 myread( ch, log, tmpbuf, 2 );
407 uint8_t id = (unsigned char)tmpbuf[0];
408 uint8_t size = (unsigned char)tmpbuf[1];
409 // cout << "message = " << (int)id << " size = " << (int)size << endl;
412 if ( ch->get_type() == sgFileType ) {
413 int count = myread( ch, log, savebuf, size );
414 if ( count != size ) {
415 cout << "ERROR: didn't read enough bytes!" << endl;
418 #ifdef READ_ONE_BY_ONE
419 for ( int i = 0; i < size; ++i ) {
420 myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
423 myread( ch, log, savebuf, size );
428 myread( ch, log, tmpbuf, 2 );
429 uint8_t cksum0 = (unsigned char)tmpbuf[0];
430 uint8_t cksum1 = (unsigned char)tmpbuf[1];
432 if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
434 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
439 cout << "Check sum failure!" << endl;
444 // load the next message of a real time data stream
445 int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
446 gps *gpspacket, imu *imupacket, nav *navpacket,
447 servo *servopacket, health *healthpacket,
448 bool ignore_checksum )
454 // cout << "in next_message()" << endl;
458 // scan for sync characters
460 uint8_t sync0, sync1;
461 result = serial_read( serial, log, tmpbuf, 2 );
462 sync0 = (unsigned char)tmpbuf[0];
463 sync1 = (unsigned char)tmpbuf[1];
464 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
467 serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
468 // cout << "scanning for start of message "
469 // << (unsigned int)sync0 << " " << (unsigned int)sync1
473 if ( scan_count > 0 ) {
474 cout << "found start of message after discarding " << scan_count
477 // cout << "found start of message ..." << endl;
479 // read message id and size
480 serial_read( serial, log, tmpbuf, 2 );
481 uint8_t id = (unsigned char)tmpbuf[0];
482 uint8_t size = (unsigned char)tmpbuf[1];
483 // cout << "message = " << (int)id << " size = " << (int)size << endl;
486 serial_read( serial, log, savebuf, size );
489 serial_read( serial, log, tmpbuf, 2 );
490 uint8_t cksum0 = (unsigned char)tmpbuf[0];
491 uint8_t cksum1 = (unsigned char)tmpbuf[1];
492 // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
495 if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
497 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
503 cout << "Check sum failure!" << endl;
510 static double interp( double a, double b, double p, bool rotational = false ) {
513 // special handling of rotational data
514 if ( diff > SGD_PI ) {
516 } else if ( diff < -SGD_PI ) {
524 gps UGEARInterpGPS( const gps A, const gps B, const double percent )
527 p.time = interp(A.time, B.time, percent);
528 p.lat = interp(A.lat, B.lat, percent);
529 p.lon = interp(A.lon, B.lon, percent);
530 p.alt = interp(A.alt, B.alt, percent);
531 p.ve = interp(A.ve, B.ve, percent);
532 p.vn = interp(A.vn, B.vn, percent);
533 p.vd = interp(A.vd, B.vd, percent);
534 p.ITOW = (int)interp(A.ITOW, B.ITOW, percent);
535 p.err_type = A.err_type;
540 imu UGEARInterpIMU( const imu A, const imu B, const double percent )
543 p.time = interp(A.time, B.time, percent);
544 p.p = interp(A.p, B.p, percent);
545 p.q = interp(A.q, B.q, percent);
546 p.r = interp(A.r, B.r, percent);
547 p.ax = interp(A.ax, B.ax, percent);
548 p.ay = interp(A.ay, B.ay, percent);
549 p.az = interp(A.az, B.az, percent);
550 p.hx = interp(A.hx, B.hx, percent);
551 p.hy = interp(A.hy, B.hy, percent);
552 p.hz = interp(A.hz, B.hz, percent);
553 p.Ps = interp(A.Ps, B.Ps, percent);
554 p.Pt = interp(A.Pt, B.Pt, percent);
555 p.phi = interp(A.phi, B.phi, percent, true);
556 p.the = interp(A.the, B.the, percent, true);
557 p.psi = interp(A.psi, B.psi, percent, true);
558 p.err_type = A.err_type;
563 nav UGEARInterpNAV( const nav A, const nav B, const double percent )
566 p.time = interp(A.time, B.time, percent);
567 p.lat = interp(A.lat, B.lat, percent);
568 p.lon = interp(A.lon, B.lon, percent);
569 p.alt = interp(A.alt, B.alt, percent);
570 p.ve = interp(A.ve, B.ve, percent);
571 p.vn = interp(A.vn, B.vn, percent);
572 p.vd = interp(A.vd, B.vd, percent);
573 p.err_type = A.err_type;
579 servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
582 for ( int i = 0; i < 8; ++i ) {
583 p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent);
591 health UGEARInterpHEALTH( const health A, const health B, const double percent )
594 p.volts_raw = interp(A.volts_raw, B.volts_raw, percent);
595 p.volts = interp(A.volts, B.volts, percent);
596 p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
597 p.time = interp(A.time, B.time, percent);