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)(uint8_t)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,
87 health *healthpacket )
89 if ( id == GPS_PACKET ) {
90 *gpspacket = *(struct gps *)buf;
91 gpspacket->lat = sg_swap_double( (uint8_t *)buf, 0 );
92 gpspacket->lon = sg_swap_double( (uint8_t *)buf, 8 );
93 gpspacket->alt = sg_swap_double( (uint8_t *)buf, 16 );
94 gpspacket->vn = sg_swap_double( (uint8_t *)buf, 24 );
95 gpspacket->ve = sg_swap_double( (uint8_t *)buf, 32 );
96 gpspacket->vd = sg_swap_double( (uint8_t *)buf, 40 );
97 gpspacket->time = sg_swap_double( (uint8_t *)buf, 52 );
98 } else if ( id == IMU_PACKET ) {
99 *imupacket = *(struct imu *)buf;
100 imupacket->p = sg_swap_double( (uint8_t *)buf, 0 );
101 imupacket->q = sg_swap_double( (uint8_t *)buf, 8 );
102 imupacket->r = sg_swap_double( (uint8_t *)buf, 16 );
103 imupacket->ax = sg_swap_double( (uint8_t *)buf, 24 );
104 imupacket->ay = sg_swap_double( (uint8_t *)buf, 32 );
105 imupacket->az = sg_swap_double( (uint8_t *)buf, 40 );
106 imupacket->hx = sg_swap_double( (uint8_t *)buf, 48 );
107 imupacket->hy = sg_swap_double( (uint8_t *)buf, 56 );
108 imupacket->hz = sg_swap_double( (uint8_t *)buf, 64 );
109 imupacket->Ps = sg_swap_double( (uint8_t *)buf, 72 );
110 imupacket->Pt = sg_swap_double( (uint8_t *)buf, 80 );
111 imupacket->phi = sg_swap_double( (uint8_t *)buf, 88 );
112 imupacket->the = sg_swap_double( (uint8_t *)buf, 96 );
113 imupacket->psi = sg_swap_double( (uint8_t *)buf, 104 );
114 imupacket->time = sg_swap_double( (uint8_t *)buf, 116 );
115 // printf("imu.time = %.4f\n", imupacket->time);
116 } else if ( id == NAV_PACKET ) {
117 *navpacket = *(struct nav *)buf;
118 navpacket->lon = sg_swap_double( (uint8_t *)buf, 0 );
119 navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
120 navpacket->alt = sg_swap_double( (uint8_t *)buf, 16 );
121 navpacket->vn = sg_swap_double( (uint8_t *)buf, 24 );
122 navpacket->ve = sg_swap_double( (uint8_t *)buf, 32 );
123 navpacket->vd = sg_swap_double( (uint8_t *)buf, 40 );
124 navpacket->time = sg_swap_double( (uint8_t *)buf, 52 );
125 } else if ( id == SERVO_PACKET ) {
126 *servopacket = *(struct servo *)buf;
127 servopacket->time = sg_swap_double( (uint8_t *)buf, 20 );
128 // printf("servo time = %.3f\n", servopacket->time);
129 } else if ( id == HEALTH_PACKET ) {
130 *healthpacket = *(struct health *)buf;
131 healthpacket->time = sg_swap_double( (uint8_t *)buf, 16 );
133 cout << "unknown id = " << id << endl;
138 // load the specified file, return the number of records loaded
139 bool UGEARTrack::load( const string &file ) {
151 double servo_time = 0;
152 double health_time = 0;
161 SGFile input( file );
162 if ( !input.open( SG_IO_IN ) ) {
163 cout << "Cannot open file: " << file << endl;
167 while ( ! input.eof() ) {
168 // cout << "looking for next message ..." << endl;
169 int id = next_message( &input, NULL, &gpspacket, &imupacket,
170 &navpacket, &servopacket, &healthpacket );
173 if ( id == GPS_PACKET ) {
174 if ( gpspacket.time > gps_time ) {
175 gps_data.push_back( gpspacket );
176 gps_time = gpspacket.time;
178 cout << "oops gps back in time: " << gpspacket.time << " " << gps_time << endl;
180 } else if ( id == IMU_PACKET ) {
181 if ( imupacket.time > imu_time ) {
182 imu_data.push_back( imupacket );
183 imu_time = imupacket.time;
185 cout << "oops imu back in time" << endl;
187 } else if ( id == NAV_PACKET ) {
188 if ( navpacket.time > nav_time ) {
189 nav_data.push_back( navpacket );
190 nav_time = navpacket.time;
192 cout << "oops nav back in time" << endl;
194 } else if ( id == SERVO_PACKET ) {
195 if ( servopacket.time > servo_time ) {
196 servo_data.push_back( servopacket );
197 servo_time = servopacket.time;
199 cout << "oops servo back in time" << endl;
201 } else if ( id == HEALTH_PACKET ) {
202 if ( healthpacket.time > health_time ) {
203 health_data.push_back( healthpacket );
204 health_time = healthpacket.time;
206 cout << "oops health back in time" << endl;
211 cout << "processed " << count << " messages" << endl;
216 // attempt to work around some system dependent issues. Our read can
217 // return < data than we want.
218 int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
222 result = ch->read( buf, length );
223 // cout << "wanted " << length << " read " << result << " bytes" << endl;
224 if ( ch->get_type() == sgFileType ) {
225 myeof = ((SGFile *)ch)->eof();
229 if ( result > 0 && log != NULL ) {
230 log->write( buf, result );
236 // attempt to work around some system dependent issues. Our read can
237 // return < data than we want.
238 int serial_read( SGSerialPort *serial, SGIOChannel *log,
239 char *buf, int length )
245 while ( bytes_read < length ) {
246 result = serial->read_port( tmp, length - bytes_read );
247 if ( result > 0 && log != NULL ) {
248 log->write( buf, result );
250 bytes_read += result;
252 // cout << " read " << bytes_read << " of " << length << endl;
258 // load the next message of a real time data stream
259 int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
260 gps *gpspacket, imu *imupacket, nav *navpacket,
261 servo *servopacket, health *healthpacket )
266 // cout << "in next_message()" << endl;
270 // scan for sync characters
271 uint8_t sync0, sync1;
272 myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
273 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
274 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
276 myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
277 // cout << "scanning for start of message "
278 // << (unsigned int)sync0 << " " << (unsigned int)sync1
279 // << ", eof = " << ch->eof() << endl;
280 if ( ch->get_type() == sgFileType ) {
281 myeof = ((SGFile *)ch)->eof();
285 cout << "found start of message ..." << endl;
287 // read message id and size
288 myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
289 myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
290 // cout << "message = " << (int)id << " size = " << (int)size << endl;
293 if ( ch->get_type() == sgFileType ) {
294 int count = myread( ch, log, savebuf, size );
295 if ( count != size ) {
296 cout << "ERROR: didn't read enough bytes!" << endl;
299 #ifdef READ_ONE_BY_ONE
300 for ( int i = 0; i < size; ++i ) {
301 myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
304 myread( ch, log, savebuf, size );
309 myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
310 myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
312 if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
313 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
318 cout << "Check sum failure!" << endl;
323 // load the next message of a real time data stream
324 int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
325 gps *gpspacket, imu *imupacket, nav *navpacket,
326 servo *servopacket, health *healthpacket )
332 // cout << "in next_message()" << endl;
336 // scan for sync characters
337 uint8_t sync0, sync1;
338 result = serial_read( serial, log, tmpbuf, 2 );
339 sync0 = (unsigned char)tmpbuf[0];
340 sync1 = (unsigned char)tmpbuf[1];
341 while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
343 serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
344 cout << "scanning for start of message "
345 << (unsigned int)sync0 << " " << (unsigned int)sync1
349 // cout << "found start of message ..." << endl;
351 // read message id and size
352 serial_read( serial, log, tmpbuf, 2 );
353 uint8_t id = (unsigned char)tmpbuf[0];
354 uint8_t size = (unsigned char)tmpbuf[1];
355 // cout << "message = " << (int)id << " size = " << (int)size << endl;
358 serial_read( serial, log, savebuf, size );
361 serial_read( serial, log, tmpbuf, 2 );
362 uint8_t cksum0 = (unsigned char)tmpbuf[0];
363 uint8_t cksum1 = (unsigned char)tmpbuf[1];
364 // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
367 if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
368 parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
374 cout << "Check sum failure!" << endl;
381 static double interp( double a, double b, double p, bool rotational = false ) {
384 // special handling of rotational data
385 if ( diff > SGD_PI ) {
387 } else if ( diff < -SGD_PI ) {
395 gps UGEARInterpGPS( const gps A, const gps B, const double percent )
398 p.time = interp(A.time, B.time, percent);
399 p.lat = interp(A.lat, B.lat, percent);
400 p.lon = interp(A.lon, B.lon, percent);
401 p.alt = interp(A.alt, B.alt, percent);
402 p.ve = interp(A.ve, B.ve, percent);
403 p.vn = interp(A.vn, B.vn, percent);
404 p.vd = interp(A.vd, B.vd, percent);
405 p.ITOW = (int)interp(A.ITOW, B.ITOW, percent);
406 p.err_type = A.err_type;
411 imu UGEARInterpIMU( const imu A, const imu B, const double percent )
414 p.time = interp(A.time, B.time, percent);
415 p.p = interp(A.p, B.p, percent);
416 p.q = interp(A.q, B.q, percent);
417 p.r = interp(A.r, B.r, percent);
418 p.ax = interp(A.ax, B.ax, percent);
419 p.ay = interp(A.ay, B.ay, percent);
420 p.az = interp(A.az, B.az, percent);
421 p.hx = interp(A.hx, B.hx, percent);
422 p.hy = interp(A.hy, B.hy, percent);
423 p.hz = interp(A.hz, B.hz, percent);
424 p.Ps = interp(A.Ps, B.Ps, percent);
425 p.Pt = interp(A.Pt, B.Pt, percent);
426 p.phi = interp(A.phi, B.phi, percent);
427 p.the = interp(A.the, B.the, percent);
428 p.psi = interp(A.psi, B.psi, percent);
429 p.err_type = A.err_type;
434 nav UGEARInterpNAV( const nav A, const nav B, const double percent )
437 p.time = interp(A.time, B.time, percent);
438 p.lat = interp(A.lat, B.lat, percent);
439 p.lon = interp(A.lon, B.lon, percent);
440 p.alt = interp(A.alt, B.alt, percent);
441 p.ve = interp(A.ve, B.ve, percent);
442 p.vn = interp(A.vn, B.vn, percent);
443 p.vd = interp(A.vd, B.vd, percent);
444 p.err_type = A.err_type;
450 servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
453 for ( int i = 0; i < 8; ++i ) {
454 p.chn[i] = (uint16_t)interp(A.chn[i], B.chn[i], percent);
462 health UGEARInterpHEALTH( const health A, const health B, const double percent )
465 p.volts_raw = interp(A.volts_raw, B.volts_raw, percent);
466 p.volts = interp(A.volts, B.volts, percent);
467 p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
468 p.time = interp(A.time, B.time, percent);