8 #include <simgear/constants.h>
9 #include <simgear/io/sg_file.hxx>
10 #include <simgear/math/sg_geodesy.hxx>
11 #include <simgear/misc/sg_path.hxx>
12 #include <simgear/misc/sgstream.hxx>
13 #include <simgear/misc/strutils.hxx>
14 #include <simgear/misc/stdint.hxx>
22 #define START_OF_MSG0 147
23 #define START_OF_MSG1 224
31 UGTrack::~UGTrack() {};
34 // swap the 1st 4 bytes with the last 4 bytes of a stargate double so
35 // it matches the PC representation
36 static double sg_swap_double( uint8_t *buf, size_t offset ) {
40 for ( size_t i = 0; i < 4; ++i ) {
41 tmpbuf[i] = buf[offset + i + 4];
43 for ( size_t i = 0; i < 4; ++i ) {
44 tmpbuf[i + 4] = buf[offset + i];
47 // for ( size_t i = 0; i < 8; ++i ) {
48 // printf("%d ", tmpbuf[i]);
52 result = (double *)tmpbuf;
57 static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
58 uint8_t cksum0, uint8_t cksum1,
59 bool ignore_checksum )
61 if ( ignore_checksum ) {
70 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
74 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
76 for ( uint8_t i = 0; i < size; i++ ) {
77 c0 += (uint8_t)buf[i];
79 // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
80 // << " [" << (unsigned int)(uint8_t)buf[i] << "]" << endl;
83 // cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
84 // << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
87 if ( c0 == cksum0 && c1 == cksum1 ) {
95 void UGTrack::parse_msg( const int id, char *buf,
96 struct gps *gpspacket, imu *imupacket,
97 nav *navpacket, servo *servopacket,
98 health *healthpacket )
100 if ( id == GPS_PACKET ) {
101 *gpspacket = *(struct gps *)buf;
103 gpspacket->time = sg_swap_double( (uint8_t *)buf, 0 );
104 gpspacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
105 gpspacket->lon = sg_swap_double( (uint8_t *)buf, 16 );
106 gpspacket->alt = sg_swap_double( (uint8_t *)buf, 24 );
107 gpspacket->vn = sg_swap_double( (uint8_t *)buf, 32 );
108 gpspacket->ve = sg_swap_double( (uint8_t *)buf, 40 );
109 gpspacket->vd = sg_swap_double( (uint8_t *)buf, 48 );
110 gpspacket->ITOW = sg_swap_double( (uint8_t *)buf, 56 );
112 } else if ( id == IMU_PACKET ) {
113 *imupacket = *(struct imu *)buf;
115 imupacket->time = sg_swap_double( (uint8_t *)buf, 0 );
116 imupacket->p = sg_swap_double( (uint8_t *)buf, 8 );
117 imupacket->q = sg_swap_double( (uint8_t *)buf, 16 );
118 imupacket->r = sg_swap_double( (uint8_t *)buf, 24 );
119 imupacket->ax = sg_swap_double( (uint8_t *)buf, 32 );
120 imupacket->ay = sg_swap_double( (uint8_t *)buf, 40 );
121 imupacket->az = sg_swap_double( (uint8_t *)buf, 48 );
122 imupacket->hx = sg_swap_double( (uint8_t *)buf, 56 );
123 imupacket->hy = sg_swap_double( (uint8_t *)buf, 64 );
124 imupacket->hz = sg_swap_double( (uint8_t *)buf, 72 );
125 imupacket->Ps = sg_swap_double( (uint8_t *)buf, 80 );
126 imupacket->Pt = sg_swap_double( (uint8_t *)buf, 88 );
127 imupacket->phi = sg_swap_double( (uint8_t *)buf, 96 );
128 imupacket->the = sg_swap_double( (uint8_t *)buf, 104 );
129 imupacket->psi = sg_swap_double( (uint8_t *)buf, 112 );
131 // printf("imu.time = %.4f size = %d\n", imupacket->time, sizeof(struct imu));
132 } else if ( id == NAV_PACKET ) {
133 *navpacket = *(struct nav *)buf;
135 navpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
136 navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
137 navpacket->lon = sg_swap_double( (uint8_t *)buf, 16 );
138 navpacket->alt = sg_swap_double( (uint8_t *)buf, 24 );
139 navpacket->vn = sg_swap_double( (uint8_t *)buf, 32 );
140 navpacket->ve = sg_swap_double( (uint8_t *)buf, 40 );
141 navpacket->vd = sg_swap_double( (uint8_t *)buf, 48 );
143 } else if ( id == SERVO_PACKET ) {
144 *servopacket = *(struct servo *)buf;
146 servopacket->time = sg_swap_double( (uint8_t *)buf, 0 );
148 // printf("servo time = %.3f %d %d\n", servopacket->time, servopacket->chn[0], servopacket->chn[1]);
149 } else if ( id == HEALTH_PACKET ) {
150 *healthpacket = *(struct health *)buf;
152 healthpacket->time = sg_swap_double( (uint8_t *)buf, 0 );
155 cout << "unknown id = " << id << endl;
160 // load the named stream log file into internal buffers
161 bool UGTrack::load_stream( const string &file, bool ignore_checksum ) {
173 double servo_time = 0;
174 double health_time = 0;
183 SGFile input( file );
184 if ( !input.open( SG_IO_IN ) ) {
185 cout << "Cannot open file: " << file << endl;
189 while ( ! input.eof() ) {
190 // cout << "looking for next message ..." << endl;
191 int id = next_message( &input, NULL, &gpspacket, &imupacket,
192 &navpacket, &servopacket, &healthpacket,
196 if ( id == GPS_PACKET ) {
197 if ( gpspacket.time > gps_time ) {
198 gps_data.push_back( gpspacket );
199 gps_time = gpspacket.time;
201 cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
203 } else if ( id == IMU_PACKET ) {
204 if ( imupacket.time > imu_time ) {
205 imu_data.push_back( imupacket );
206 imu_time = imupacket.time;
208 cout << "oops imu back in time" << endl;
210 } else if ( id == NAV_PACKET ) {
211 if ( navpacket.time > nav_time ) {
212 nav_data.push_back( navpacket );
213 nav_time = navpacket.time;
215 cout << "oops nav back in time" << endl;
217 } else if ( id == SERVO_PACKET ) {
218 if ( servopacket.time > servo_time ) {
219 servo_data.push_back( servopacket );
220 servo_time = servopacket.time;
222 cout << "oops servo back in time" << endl;
224 } else if ( id == HEALTH_PACKET ) {
225 if ( healthpacket.time > health_time ) {
226 health_data.push_back( healthpacket );
227 health_time = healthpacket.time;
229 cout << "oops health back in time" << endl;
234 cout << "processed " << count << " messages" << endl;
239 // load the named stream log file into internal buffers
240 bool UGTrack::load_flight( const string &path ) {
256 gzFile fservo = NULL;
257 gzFile fhealth = NULL;
263 file = path; file.append( "gps.dat.gz" );
264 if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) {
265 printf("Cannot open %s\n", file.c_str());
269 size = sizeof( struct gps );
270 printf("gps size = %d\n", size);
271 while ( gzread( fgps, &gpspacket, size ) == size ) {
272 gps_data.push_back( gpspacket );
276 file = path; file.append( "imu.dat.gz" );
277 if ( (fimu = gzopen( file.c_str(), "r" )) == NULL ) {
278 printf("Cannot open %s\n", file.c_str());
282 size = sizeof( struct imu );
283 printf("imu size = %d\n", size);
284 while ( gzread( fimu, &imupacket, size ) == size ) {
285 imu_data.push_back( imupacket );
289 file = path; file.append( "nav.dat.gz" );
290 if ( (fnav = gzopen( file.c_str(), "r" )) == NULL ) {
291 printf("Cannot open %s\n", file.c_str());
295 size = sizeof( struct nav );
296 printf("nav size = %d\n", size);
297 while ( gzread( fnav, &navpacket, size ) == size ) {
298 // printf("%.4f %.4f\n", navpacket.lat, navpacket.lon);
299 nav_data.push_back( navpacket );
302 // open the servo file
303 file = path; file.append( "servo.dat.gz" );
304 if ( (fservo = gzopen( file.c_str(), "r" )) == NULL ) {
305 printf("Cannot open %s\n", file.c_str());
309 size = sizeof( struct servo );
310 printf("servo size = %d\n", size);
311 while ( gzread( fservo, &servopacket, size ) == size ) {
312 servo_data.push_back( servopacket );
315 // open the health file
316 file = path; file.append( "health.dat.gz" );
317 if ( (fhealth = gzopen( file.c_str(), "r" )) == NULL ) {
318 printf("Cannot open %s\n", file.c_str());
322 size = sizeof( struct health );
323 printf("health size = %d\n", size);
324 while ( gzread( fhealth, &healthpacket, size ) == size ) {
325 health_data.push_back( healthpacket );
332 // attempt to work around some system dependent issues. Our read can
333 // return < data than we want.
334 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
338 result = ch->read( buf, length );
339 // cout << "wanted " << length << " read " << result << " bytes" << endl;
340 if ( ch->get_type() == sgFileType ) {
341 myeof = ((SGFile *)ch)->eof();
345 if ( result > 0 && log != NULL ) {
346 log->write( buf, result );
352 // attempt to work around some system dependent issues. Our read can
353 // return < data than we want.
354 int serial_read( SGSerialPort *serial, SGIOChannel *log,
355 char *buf, int length )
361 while ( bytes_read < length ) {
362 result = serial->read_port( tmp, length - bytes_read );
363 bytes_read += result;
365 // cout << " read " << bytes_read << " of " << length << endl;
368 if ( bytes_read > 0 && log != NULL ) {
369 log->write( buf, bytes_read );
376 // load the next message of a real time data stream
377 int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
378 gps *gpspacket, imu *imupacket, nav *navpacket,
379 servo *servopacket, health *healthpacket,
380 bool ignore_checksum )
385 // cout << "in next_message()" << endl;
389 // scan for sync characters
390 uint8_t sync0, sync1;
391 myread( ch, log, tmpbuf, 2 );
392 sync0 = (unsigned char)tmpbuf[0];
393 sync1 = (unsigned char)tmpbuf[1];
394 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
396 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
397 cout << "scanning for start of message "
398 << (unsigned int)sync0 << " " << (unsigned int)sync1
399 << ", eof = " << ch->eof() << endl;
400 if ( ch->get_type() == sgFileType ) {
401 myeof = ((SGFile *)ch)->eof();
405 cout << "found start of message ..." << endl;
407 // read message id and size
408 myread( ch, log, tmpbuf, 2 );
409 uint8_t id = (unsigned char)tmpbuf[0];
410 uint8_t size = (unsigned char)tmpbuf[1];
411 // cout << "message = " << (int)id << " size = " << (int)size << endl;
414 if ( ch->get_type() == sgFileType ) {
415 int count = myread( ch, log, savebuf, size );
416 if ( count != size ) {
417 cout << "ERROR: didn't read enough bytes!" << endl;
420 #ifdef READ_ONE_BY_ONE
421 for ( int i = 0; i < size; ++i ) {
422 myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
425 myread( ch, log, savebuf, size );
430 myread( ch, log, tmpbuf, 2 );
431 uint8_t cksum0 = (unsigned char)tmpbuf[0];
432 uint8_t cksum1 = (unsigned char)tmpbuf[1];
434 if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
436 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
441 cout << "Check sum failure!" << endl;
446 // load the next message of a real time data stream
447 int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
448 gps *gpspacket, imu *imupacket, nav *navpacket,
449 servo *servopacket, health *healthpacket,
450 bool ignore_checksum )
455 // cout << "in next_message()" << endl;
459 // scan for sync characters
461 uint8_t sync0, sync1;
462 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);