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 std::string fdata = file.local8BitStr();
266 if ( (fgps = gzopen( fdata.c_str(), "r" )) == NULL ) {
267 printf("Cannot open %s\n", fdata.c_str());
271 size = sizeof( struct gps );
272 printf("gps size = %d\n", size);
273 while ( gzread( fgps, &gpspacket, size ) == size ) {
274 gps_data.push_back( gpspacket );
278 file = path; file.append( "imu.dat.gz" );
279 fdata = file.local8BitStr();
280 if ( (fimu = gzopen( fdata.c_str(), "r" )) == NULL ) {
281 printf("Cannot open %s\n", fdata.c_str());
285 size = sizeof( struct imu );
286 printf("imu size = %d\n", size);
287 while ( gzread( fimu, &imupacket, size ) == size ) {
288 imu_data.push_back( imupacket );
292 file = path; file.append( "nav.dat.gz" );
293 fdata = file.local8BitStr();
295 if ( (fnav = gzopen( fdata.c_str(), "r" )) == NULL ) {
296 printf("Cannot open %s\n", fdata.c_str());
300 size = sizeof( struct nav );
301 printf("nav size = %d\n", size);
302 while ( gzread( fnav, &navpacket, size ) == size ) {
303 // printf("%.4f %.4f\n", navpacket.lat, navpacket.lon);
304 nav_data.push_back( navpacket );
307 // open the servo file
308 file = path; file.append( "servo.dat.gz" );
309 fdata = file.local8BitStr();
311 if ( (fservo = gzopen( fdata.c_str(), "r" )) == NULL ) {
312 printf("Cannot open %s\n", fdata.c_str());
316 size = sizeof( struct servo );
317 printf("servo size = %d\n", size);
318 while ( gzread( fservo, &servopacket, size ) == size ) {
319 servo_data.push_back( servopacket );
322 // open the health file
323 file = path; file.append( "health.dat.gz" );
324 fdata = file.local8BitStr();
326 if ( (fhealth = gzopen( fdata.c_str(), "r" )) == NULL ) {
327 printf("Cannot open %s\n", fdata.c_str());
331 size = sizeof( struct health );
332 printf("health size = %d\n", size);
333 while ( gzread( fhealth, &healthpacket, size ) == size ) {
334 health_data.push_back( healthpacket );
341 // attempt to work around some system dependent issues. Our read can
342 // return < data than we want.
343 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
347 result = ch->read( buf, length );
348 // cout << "wanted " << length << " read " << result << " bytes" << endl;
349 if ( ch->get_type() == sgFileType ) {
350 myeof = ((SGFile *)ch)->eof();
354 if ( result > 0 && log != NULL ) {
355 log->write( buf, result );
361 // attempt to work around some system dependent issues. Our read can
362 // return < data than we want.
363 int serial_read( SGSerialPort *serial, SGIOChannel *log,
364 char *buf, int length )
370 while ( bytes_read < length ) {
371 result = serial->read_port( tmp, length - bytes_read );
372 bytes_read += result;
374 // cout << " read " << bytes_read << " of " << length << endl;
377 if ( bytes_read > 0 && log != NULL ) {
378 log->write( buf, bytes_read );
385 // load the next message of a real time data stream
386 int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
387 gps *gpspacket, imu *imupacket, nav *navpacket,
388 servo *servopacket, health *healthpacket,
389 bool ignore_checksum )
394 // cout << "in next_message()" << endl;
398 // scan for sync characters
399 uint8_t sync0, sync1;
400 myread( ch, log, tmpbuf, 2 );
401 sync0 = (unsigned char)tmpbuf[0];
402 sync1 = (unsigned char)tmpbuf[1];
403 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
405 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
406 cout << "scanning for start of message "
407 << (unsigned int)sync0 << " " << (unsigned int)sync1
408 << ", eof = " << ch->eof() << endl;
409 if ( ch->get_type() == sgFileType ) {
410 myeof = ((SGFile *)ch)->eof();
414 cout << "found start of message ..." << endl;
416 // read message id and size
417 myread( ch, log, tmpbuf, 2 );
418 uint8_t id = (unsigned char)tmpbuf[0];
419 uint8_t size = (unsigned char)tmpbuf[1];
420 // cout << "message = " << (int)id << " size = " << (int)size << endl;
423 if ( ch->get_type() == sgFileType ) {
424 int count = myread( ch, log, savebuf, size );
425 if ( count != size ) {
426 cout << "ERROR: didn't read enough bytes!" << endl;
429 #ifdef READ_ONE_BY_ONE
430 for ( int i = 0; i < size; ++i ) {
431 myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
434 myread( ch, log, savebuf, size );
439 myread( ch, log, tmpbuf, 2 );
440 uint8_t cksum0 = (unsigned char)tmpbuf[0];
441 uint8_t cksum1 = (unsigned char)tmpbuf[1];
443 if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
445 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
450 cout << "Check sum failure!" << endl;
455 // load the next message of a real time data stream
456 int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
457 gps *gpspacket, imu *imupacket, nav *navpacket,
458 servo *servopacket, health *healthpacket,
459 bool ignore_checksum )
464 // cout << "in next_message()" << endl;
468 // scan for sync characters
470 uint8_t sync0, sync1;
471 serial_read( serial, log, tmpbuf, 2 );
472 sync0 = (unsigned char)tmpbuf[0];
473 sync1 = (unsigned char)tmpbuf[1];
474 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
477 serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
478 // cout << "scanning for start of message "
479 // << (unsigned int)sync0 << " " << (unsigned int)sync1
483 if ( scan_count > 0 ) {
484 cout << "found start of message after discarding " << scan_count
487 // cout << "found start of message ..." << endl;
489 // read message id and size
490 serial_read( serial, log, tmpbuf, 2 );
491 uint8_t id = (unsigned char)tmpbuf[0];
492 uint8_t size = (unsigned char)tmpbuf[1];
493 // cout << "message = " << (int)id << " size = " << (int)size << endl;
496 serial_read( serial, log, savebuf, size );
499 serial_read( serial, log, tmpbuf, 2 );
500 uint8_t cksum0 = (unsigned char)tmpbuf[0];
501 uint8_t cksum1 = (unsigned char)tmpbuf[1];
502 // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
505 if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
507 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
513 cout << "Check sum failure!" << endl;
520 static double interp( double a, double b, double p, bool rotational = false ) {
523 // special handling of rotational data
524 if ( diff > SGD_PI ) {
526 } else if ( diff < -SGD_PI ) {
534 gps UGEARInterpGPS( const gps A, const gps B, const double percent )
537 p.time = interp(A.time, B.time, percent);
538 p.lat = interp(A.lat, B.lat, percent);
539 p.lon = interp(A.lon, B.lon, percent);
540 p.alt = interp(A.alt, B.alt, percent);
541 p.ve = interp(A.ve, B.ve, percent);
542 p.vn = interp(A.vn, B.vn, percent);
543 p.vd = interp(A.vd, B.vd, percent);
544 p.ITOW = (int)interp(A.ITOW, B.ITOW, percent);
545 p.err_type = A.err_type;
550 imu UGEARInterpIMU( const imu A, const imu B, const double percent )
553 p.time = interp(A.time, B.time, percent);
554 p.p = interp(A.p, B.p, percent);
555 p.q = interp(A.q, B.q, percent);
556 p.r = interp(A.r, B.r, percent);
557 p.ax = interp(A.ax, B.ax, percent);
558 p.ay = interp(A.ay, B.ay, percent);
559 p.az = interp(A.az, B.az, percent);
560 p.hx = interp(A.hx, B.hx, percent);
561 p.hy = interp(A.hy, B.hy, percent);
562 p.hz = interp(A.hz, B.hz, percent);
563 p.Ps = interp(A.Ps, B.Ps, percent);
564 p.Pt = interp(A.Pt, B.Pt, percent);
565 p.phi = interp(A.phi, B.phi, percent, true);
566 p.the = interp(A.the, B.the, percent, true);
567 p.psi = interp(A.psi, B.psi, percent, true);
568 p.err_type = A.err_type;
573 nav UGEARInterpNAV( const nav A, const nav B, const double percent )
576 p.time = interp(A.time, B.time, percent);
577 p.lat = interp(A.lat, B.lat, percent);
578 p.lon = interp(A.lon, B.lon, percent);
579 p.alt = interp(A.alt, B.alt, percent);
580 p.ve = interp(A.ve, B.ve, percent);
581 p.vn = interp(A.vn, B.vn, percent);
582 p.vd = interp(A.vd, B.vd, percent);
583 p.err_type = A.err_type;
589 servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
592 for ( int i = 0; i < 8; ++i ) {
593 p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent);
601 health UGEARInterpHEALTH( const health A, const health B, const double percent )
604 p.command_sequence = B.command_sequence;
605 p.time = interp(A.time, B.time, percent);