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
30 UGTrack::~UGTrack() {};
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 UGTrack::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 );
109 gpspacket->ITOW = sg_swap_double( (uint8_t *)buf, 56 );
111 } else if ( id == IMU_PACKET ) {
112 *imupacket = *(struct imu *)buf;
114 imupacket->time = sg_swap_double( (uint8_t *)buf, 0 );
115 imupacket->p = sg_swap_double( (uint8_t *)buf, 8 );
116 imupacket->q = sg_swap_double( (uint8_t *)buf, 16 );
117 imupacket->r = sg_swap_double( (uint8_t *)buf, 24 );
118 imupacket->ax = sg_swap_double( (uint8_t *)buf, 32 );
119 imupacket->ay = sg_swap_double( (uint8_t *)buf, 40 );
120 imupacket->az = sg_swap_double( (uint8_t *)buf, 48 );
121 imupacket->hx = sg_swap_double( (uint8_t *)buf, 56 );
122 imupacket->hy = sg_swap_double( (uint8_t *)buf, 64 );
123 imupacket->hz = sg_swap_double( (uint8_t *)buf, 72 );
124 imupacket->Ps = sg_swap_double( (uint8_t *)buf, 80 );
125 imupacket->Pt = sg_swap_double( (uint8_t *)buf, 88 );
126 imupacket->phi = sg_swap_double( (uint8_t *)buf, 96 );
127 imupacket->the = sg_swap_double( (uint8_t *)buf, 104 );
128 imupacket->psi = sg_swap_double( (uint8_t *)buf, 112 );
130 // printf("imu.time = %.4f size = %d\n", imupacket->time, sizeof(struct imu));
131 } else if ( id == NAV_PACKET ) {
132 *navpacket = *(struct nav *)buf;
134 navpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
135 navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
136 navpacket->lon = sg_swap_double( (uint8_t *)buf, 16 );
137 navpacket->alt = sg_swap_double( (uint8_t *)buf, 24 );
138 navpacket->vn = sg_swap_double( (uint8_t *)buf, 32 );
139 navpacket->ve = sg_swap_double( (uint8_t *)buf, 40 );
140 navpacket->vd = sg_swap_double( (uint8_t *)buf, 48 );
142 } else if ( id == SERVO_PACKET ) {
143 *servopacket = *(struct servo *)buf;
145 servopacket->time = sg_swap_double( (uint8_t *)buf, 0 );
147 // printf("servo time = %.3f %d %d\n", servopacket->time, servopacket->chn[0], servopacket->chn[1]);
148 } else if ( id == HEALTH_PACKET ) {
149 *healthpacket = *(struct health *)buf;
151 healthpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
154 cout << "unknown id = " << id << endl;
159 // load the named stream log file into internal buffers
160 bool UGTrack::load_stream( const string &file, bool ignore_checksum ) {
172 double servo_time = 0;
173 double health_time = 0;
182 SGFile input( file );
183 if ( !input.open( SG_IO_IN ) ) {
184 cout << "Cannot open file: " << file << endl;
188 while ( ! input.eof() ) {
189 // cout << "looking for next message ..." << endl;
190 int id = next_message( &input, NULL, &gpspacket, &imupacket,
191 &navpacket, &servopacket, &healthpacket,
195 if ( id == GPS_PACKET ) {
196 if ( gpspacket.time > gps_time ) {
197 gps_data.push_back( gpspacket );
198 gps_time = gpspacket.time;
200 cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
202 } else if ( id == IMU_PACKET ) {
203 if ( imupacket.time > imu_time ) {
204 imu_data.push_back( imupacket );
205 imu_time = imupacket.time;
207 cout << "oops imu back in time" << endl;
209 } else if ( id == NAV_PACKET ) {
210 if ( navpacket.time > nav_time ) {
211 nav_data.push_back( navpacket );
212 nav_time = navpacket.time;
214 cout << "oops nav back in time" << endl;
216 } else if ( id == SERVO_PACKET ) {
217 if ( servopacket.time > servo_time ) {
218 servo_data.push_back( servopacket );
219 servo_time = servopacket.time;
221 cout << "oops servo back in time" << endl;
223 } else if ( id == HEALTH_PACKET ) {
224 if ( healthpacket.time > health_time ) {
225 health_data.push_back( healthpacket );
226 health_time = healthpacket.time;
228 cout << "oops health back in time" << endl;
233 cout << "processed " << count << " messages" << endl;
238 // load the named stream log file into internal buffers
239 bool UGTrack::load_flight( const string &path ) {
255 gzFile fservo = NULL;
256 gzFile fhealth = NULL;
262 file = path; file.append( "gps.dat.gz" );
263 if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) {
264 printf("Cannot open %s\n", file.c_str());
268 size = sizeof( struct gps );
269 printf("gps size = %d\n", size);
270 while ( gzread( fgps, &gpspacket, size ) == size ) {
271 gps_data.push_back( gpspacket );
275 file = path; file.append( "imu.dat.gz" );
276 if ( (fimu = gzopen( file.c_str(), "r" )) == NULL ) {
277 printf("Cannot open %s\n", file.c_str());
281 size = sizeof( struct imu );
282 printf("imu size = %d\n", size);
283 while ( gzread( fimu, &imupacket, size ) == size ) {
284 imu_data.push_back( imupacket );
288 file = path; file.append( "nav.dat.gz" );
289 if ( (fnav = gzopen( file.c_str(), "r" )) == NULL ) {
290 printf("Cannot open %s\n", file.c_str());
294 size = sizeof( struct nav );
295 printf("nav size = %d\n", size);
296 while ( gzread( fnav, &navpacket, size ) == size ) {
297 // printf("%.4f %.4f\n", navpacket.lat, navpacket.lon);
298 nav_data.push_back( navpacket );
301 // open the servo file
302 file = path; file.append( "servo.dat.gz" );
303 if ( (fservo = gzopen( file.c_str(), "r" )) == NULL ) {
304 printf("Cannot open %s\n", file.c_str());
308 size = sizeof( struct servo );
309 printf("servo size = %d\n", size);
310 while ( gzread( fservo, &servopacket, size ) == size ) {
311 servo_data.push_back( servopacket );
314 // open the health file
315 file = path; file.append( "health.dat.gz" );
316 if ( (fhealth = gzopen( file.c_str(), "r" )) == NULL ) {
317 printf("Cannot open %s\n", file.c_str());
321 size = sizeof( struct health );
322 printf("health size = %d\n", size);
323 while ( gzread( fhealth, &healthpacket, size ) == size ) {
324 health_data.push_back( healthpacket );
331 // attempt to work around some system dependent issues. Our read can
332 // return < data than we want.
333 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
337 result = ch->read( buf, length );
338 // cout << "wanted " << length << " read " << result << " bytes" << endl;
339 if ( ch->get_type() == sgFileType ) {
340 myeof = ((SGFile *)ch)->eof();
344 if ( result > 0 && log != NULL ) {
345 log->write( buf, result );
351 // attempt to work around some system dependent issues. Our read can
352 // return < data than we want.
353 int serial_read( SGSerialPort *serial, SGIOChannel *log,
354 char *buf, int length )
360 while ( bytes_read < length ) {
361 result = serial->read_port( tmp, length - bytes_read );
362 bytes_read += result;
364 // cout << " read " << bytes_read << " of " << length << endl;
367 if ( bytes_read > 0 && log != NULL ) {
368 log->write( buf, bytes_read );
375 // load the next message of a real time data stream
376 int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
377 gps *gpspacket, imu *imupacket, nav *navpacket,
378 servo *servopacket, health *healthpacket,
379 bool ignore_checksum )
384 // cout << "in next_message()" << endl;
388 // scan for sync characters
389 uint8_t sync0, sync1;
390 myread( ch, log, tmpbuf, 2 );
391 sync0 = (unsigned char)tmpbuf[0];
392 sync1 = (unsigned char)tmpbuf[1];
393 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
395 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
396 cout << "scanning for start of message "
397 << (unsigned int)sync0 << " " << (unsigned int)sync1
398 << ", eof = " << ch->eof() << endl;
399 if ( ch->get_type() == sgFileType ) {
400 myeof = ((SGFile *)ch)->eof();
404 cout << "found start of message ..." << endl;
406 // read message id and size
407 myread( ch, log, tmpbuf, 2 );
408 uint8_t id = (unsigned char)tmpbuf[0];
409 uint8_t size = (unsigned char)tmpbuf[1];
410 // cout << "message = " << (int)id << " size = " << (int)size << endl;
413 if ( ch->get_type() == sgFileType ) {
414 int count = myread( ch, log, savebuf, size );
415 if ( count != size ) {
416 cout << "ERROR: didn't read enough bytes!" << endl;
419 #ifdef READ_ONE_BY_ONE
420 for ( int i = 0; i < size; ++i ) {
421 myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
424 myread( ch, log, savebuf, size );
429 myread( ch, log, tmpbuf, 2 );
430 uint8_t cksum0 = (unsigned char)tmpbuf[0];
431 uint8_t cksum1 = (unsigned char)tmpbuf[1];
433 if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
435 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
440 cout << "Check sum failure!" << endl;
445 // load the next message of a real time data stream
446 int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
447 gps *gpspacket, imu *imupacket, nav *navpacket,
448 servo *servopacket, health *healthpacket,
449 bool ignore_checksum )
455 // cout << "in next_message()" << endl;
459 // scan for sync characters
461 uint8_t sync0, sync1;
462 result = serial_read( serial, log, tmpbuf, 2 );
463 sync0 = (unsigned char)tmpbuf[0];
464 sync1 = (unsigned char)tmpbuf[1];
465 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
468 serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
469 // cout << "scanning for start of message "
470 // << (unsigned int)sync0 << " " << (unsigned int)sync1
474 if ( scan_count > 0 ) {
475 cout << "found start of message after discarding " << scan_count
478 // cout << "found start of message ..." << endl;
480 // read message id and size
481 serial_read( serial, log, tmpbuf, 2 );
482 uint8_t id = (unsigned char)tmpbuf[0];
483 uint8_t size = (unsigned char)tmpbuf[1];
484 // cout << "message = " << (int)id << " size = " << (int)size << endl;
487 serial_read( serial, log, savebuf, size );
490 serial_read( serial, log, tmpbuf, 2 );
491 uint8_t cksum0 = (unsigned char)tmpbuf[0];
492 uint8_t cksum1 = (unsigned char)tmpbuf[1];
493 // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
496 if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
498 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
504 cout << "Check sum failure!" << endl;
511 static double interp( double a, double b, double p, bool rotational = false ) {
514 // special handling of rotational data
515 if ( diff > SGD_PI ) {
517 } else if ( diff < -SGD_PI ) {
525 gps UGEARInterpGPS( const gps A, const gps B, const double percent )
528 p.time = interp(A.time, B.time, percent);
529 p.lat = interp(A.lat, B.lat, percent);
530 p.lon = interp(A.lon, B.lon, percent);
531 p.alt = interp(A.alt, B.alt, percent);
532 p.ve = interp(A.ve, B.ve, percent);
533 p.vn = interp(A.vn, B.vn, percent);
534 p.vd = interp(A.vd, B.vd, percent);
535 p.ITOW = (int)interp(A.ITOW, B.ITOW, percent);
536 p.err_type = A.err_type;
541 imu UGEARInterpIMU( const imu A, const imu B, const double percent )
544 p.time = interp(A.time, B.time, percent);
545 p.p = interp(A.p, B.p, percent);
546 p.q = interp(A.q, B.q, percent);
547 p.r = interp(A.r, B.r, percent);
548 p.ax = interp(A.ax, B.ax, percent);
549 p.ay = interp(A.ay, B.ay, percent);
550 p.az = interp(A.az, B.az, percent);
551 p.hx = interp(A.hx, B.hx, percent);
552 p.hy = interp(A.hy, B.hy, percent);
553 p.hz = interp(A.hz, B.hz, percent);
554 p.Ps = interp(A.Ps, B.Ps, percent);
555 p.Pt = interp(A.Pt, B.Pt, percent);
556 p.phi = interp(A.phi, B.phi, percent, true);
557 p.the = interp(A.the, B.the, percent, true);
558 p.psi = interp(A.psi, B.psi, percent, true);
559 p.err_type = A.err_type;
564 nav UGEARInterpNAV( const nav A, const nav B, const double percent )
567 p.time = interp(A.time, B.time, percent);
568 p.lat = interp(A.lat, B.lat, percent);
569 p.lon = interp(A.lon, B.lon, percent);
570 p.alt = interp(A.alt, B.alt, percent);
571 p.ve = interp(A.ve, B.ve, percent);
572 p.vn = interp(A.vn, B.vn, percent);
573 p.vd = interp(A.vd, B.vd, percent);
574 p.err_type = A.err_type;
580 servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
583 for ( int i = 0; i < 8; ++i ) {
584 p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent);
592 health UGEARInterpHEALTH( const health A, const health B, const double percent )
595 p.command_sequence = B.command_sequence;
596 p.time = interp(A.time, B.time, percent);