-#include <iostream>
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
+
+#include <simgear/compiler.h>
-#include <plib/ul.h>
+#include <iostream>
#include <simgear/constants.h>
+#include <simgear/io/sg_file.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/misc/strutils.hxx>
+#include <simgear/misc/stdint.hxx>
#include "MIDG-II.hxx"
-SG_USING_STD(cout);
-SG_USING_STD(endl);
+using std::cout;
+using std::endl;
MIDGTrack::MIDGTrack() {};
MIDGTrack::~MIDGTrack() {};
-
-
+/*
+ * Unused function
+ */
+#if(0)
static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
uint32_t result = 0;
char *ptr = buf + offset;
// MIDG data is big endian so swap if needed.
- if ( ulIsLittleEndian ) {
+ if ( sgIsLittleEndian() ) {
if ( size == 4 ) {
- ulEndianSwap( (uint32_t *)ptr );
+ sgEndianSwap( (uint32_t *)ptr );
} else if ( size == 2 ) {
- ulEndianSwap( (uint16_t *)ptr );
+ sgEndianSwap( (uint16_t *)ptr );
}
}
return result;
}
+#endif
static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
c0 += id;
c1 += c0;
+ // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
c0 += size;
c1 += c0;
+ // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1 << endl;
for ( uint8_t i = 0; i < size; i++ ) {
c0 += (uint8_t)buf[i];
c1 += c0;
+ // cout << "c0 = " << (unsigned int)c0 << " c1 = " << (unsigned int)c1
+ // << " [" << (unsigned int)buf[i] << "]" << endl;
}
// cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
{
+/*
+ * Completely unused parser results. Removed from compiling to remove the warnings
+ */
+#if(0)
if ( id == 1 ) {
uint32_t ts;
uint16_t status;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl;
-
+
// status
status = (uint16_t)read_swab( buf, 4, 2 );
// cout << " status = " << status << endl;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
- // cout << " time stamp = " << ts << endl;
- if ( ts > att->get_msec() && att->get_msec() > 1.0 ) {
- attdata.push_back( *att );
- }
- if ( ts < att->get_msec() ) {
- cout << "OOOPS moving back in time!!! " << ts << " < " << att->get_msec() << endl;
- } else {
- att->midg_time = MIDGTime( ts );
- }
+ // cout << " att time stamp = " << ts << endl;
+ att->midg_time = MIDGTime( ts );
// p, q, r
p = (int16_t)read_swab( buf, 4, 2 );
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
- // cout << " time stamp = " << ts << endl;
- if ( ts > pos->get_msec() && pos->get_msec() > 1.0 ) {
- posdata.push_back( *pos );
- }
- if ( ts < pos->get_msec() ) {
- cout << "OOOPS moving back in time!!! " << ts << " < " << pos->get_msec() << endl;
- } else {
- pos->midg_time = MIDGTime( ts );
- }
+ // cout << " pos time stamp = " << ts << endl;
+ pos->midg_time = MIDGTime( ts );
// posx, posy, posz
posx = (int32_t)read_swab( buf, 4, 4 );
// cout << " pos = " << posx << "," << posy << "," << posz << endl;
double xyz[3];
- xyz[0] = posx/100; xyz[1] = posy/100; xyz[2] = posz/100;
+ xyz[0] = (double)posx/100; xyz[1] = (double)posy/100; xyz[2] = (double)posz/100;
double lat, lon, alt;
sgCartToGeod(xyz, &lat, &lon, &alt);
pos->lat_deg = lat * 180.0 / SG_PI;
vely = (int32_t)read_swab( buf, 20, 4 );
velz = (int32_t)read_swab( buf, 24, 4 );
// cout << " vel = " << velx << "," << vely << "," << velz << endl;
+ double tmp1 = velx*velx + vely*vely + velz*velz;
+ double vel_cms = sqrt( tmp1 );
+ double vel_ms = vel_cms / 100.0;
+ pos->speed_kts = vel_ms * SG_METER_TO_NM * 3600;
// flags
flags = (uint8_t)read_swab( buf, 28, 1 );
// previous data or not, just roll it into the current data
// independent of time stamp.
gps_ts = (uint32_t)read_swab( buf, 0, 4 );
- // if ( ts > pt->get_msec() && pt->get_msec() > 1.0 ) {
- // data.push_back( *pt );
- // }
- // if ( ts < pt->get_msec() ) {
- // cout << "OOOPS moving back in time!!! " << ts << " < " << pt->get_msec() << endl;
- // } else {
- // pt->midg_time = MIDGTime( ts );
- // }
+ // pt->midg_time = MIDGTime( ts );
gps_week = (uint16_t)read_swab( buf, 4, 2 );
// cout << " gps time stamp = " << gps_ts << " week = " << gps_week
// position dop
pdop = (uint16_t)read_swab( buf, 32, 2 );
// cout << " pdop = " << pdop << endl;
-
+
// position accuracy
pacc = (uint16_t)read_swab( buf, 34, 2 );
// cout << " pacc = " << pacc << endl;
-
+
// speed accuracy
sacc = (uint16_t)read_swab( buf, 36, 2 );
// cout << " sacc = " << sacc << endl;
-
+
} else {
cout << "unknown id = " << id << endl;
}
+#endif
}
// load the specified file, return the number of records loaded
-int MIDGTrack::load( const string &file ) {
+bool MIDGTrack::load( const string &file ) {
int count = 0;
- posdata.clear();
- attdata.clear();
+ MIDGpos pos;
+ MIDGatt att;
+
+ uint32_t pos_time = 1;
+ uint32_t att_time = 1;
- // openg the file
- fd = fopen( file.c_str(), "r" );
+ pos_data.clear();
+ att_data.clear();
- if ( fd == NULL ) {
+ // open the file
+ SGFile input( file );
+ if ( !input.open( SG_IO_IN ) ) {
cout << "Cannot open file: " << file << endl;
- return 0;
+ return false;
}
- vector <string> tokens;
- MIDGpos pos;
- MIDGatt att;
+ while ( ! input.eof() ) {
+ // cout << "looking for next message ..." << endl;
+ int id = next_message( &input, NULL, &pos, &att );
+ count++;
+
+ if ( id == 10 ) {
+ if ( att.get_msec() > att_time ) {
+ att_data.push_back( att );
+ att_time = att.get_msec();
+ } else {
+ cout << "oops att back in time" << endl;
+ }
+ } else if ( id == 12 ) {
+ if ( pos.get_msec() > pos_time ) {
+ pos_data.push_back( pos );
+ pos_time = pos.get_msec();
+ } else {
+ cout << "oops pos back in time" << endl;
+ }
+ }
+ }
+
+ cout << "processed " << count << " messages" << endl;
+ return true;
+}
+
+
+// attempt to work around some system dependent issues. Our read can
+// return < data than we want.
+int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
+ bool myeof = false;
+ int result = 0;
+ if ( !myeof ) {
+ result = ch->read( buf, length );
+ // cout << "wanted " << length << " read " << result << " bytes" << endl;
+ if ( ch->get_type() == sgFileType ) {
+ myeof = ((SGFile *)ch)->eof();
+ }
+ }
+
+ if ( result > 0 && log != NULL ) {
+ log->write( buf, result );
+ }
+
+ return result;
+}
+
+// attempt to work around some system dependent issues. Our read can
+// return < data than we want.
+int serial_read( SGSerialPort *serial, char *buf, int length ) {
+ int result = 0;
+ int bytes_read = 0;
+ char *tmp = buf;
+
+ while ( bytes_read < length ) {
+ result = serial->read_port( tmp, length - bytes_read );
+ bytes_read += result;
+ tmp += result;
+ // cout << " read " << bytes_read << " of " << length << endl;
+ }
- while ( ! feof( fd ) ) {
- // scan for sync characters
- int sync0, sync1;
- sync0 = fgetc( fd );
- sync1 = fgetc( fd );
- while ( (sync0 != 129 || sync1 != 161) && !feof(fd) ) {
- sync0 = sync1;
- sync1 = fgetc( fd );
+ return bytes_read;
+}
+
+// load the next message of a real time data stream
+int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
+ MIDGpos *pos, MIDGatt *att )
+{
+ char tmpbuf[256];
+ char savebuf[256];
+
+ // cout << "in next_message()" << endl;
+
+ bool myeof = false;
+
+ // scan for sync characters
+ uint8_t sync0, sync1;
+ myread( ch, log, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
+ myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+ while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
+ sync0 = sync1;
+ myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+ // cout << "scanning for start of message "
+ // << (unsigned int)sync0 << " " << (unsigned int)sync1
+ // << ", eof = " << ch->eof() << endl;
+ if ( ch->get_type() == sgFileType ) {
+ myeof = ((SGFile *)ch)->eof();
}
+ }
- // cout << "start of message ..." << endl;
-
- // read message id and size
- int id = fgetc( fd );
- int size = fgetc( fd );
- // cout << "message = " << id << " size = " << size << endl;
-
- // load message
- char buf[256];
- fread( buf, size, 1, fd );
-
- // read checksum
- int cksum0 = fgetc( fd );
- int cksum1 = fgetc( fd );
-
- if ( validate_cksum( id, size, buf, cksum0, cksum1 ) ) {
- parse_msg( id, buf, &pos, &att );
- } else {
- cout << "Check sum failure!" << endl;
+ // cout << "found start of message ..." << endl;
+
+ // read message id and size
+ myread( ch, log, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
+ myread( ch, log, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
+ // cout << "message = " << (int)id << " size = " << (int)size << endl;
+
+ // load message
+ if ( ch->get_type() == sgFileType ) {
+ int count = myread( ch, log, savebuf, size );
+ if ( count != size ) {
+ cout << "ERROR: didn't read enough bytes!" << endl;
+ }
+ } else {
+#ifdef READ_ONE_BY_ONE
+ for ( int i = 0; i < size; ++i ) {
+ myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
}
+#else
+ myread( ch, log, savebuf, size );
+#endif
}
- return count;
+ // read checksum
+ myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
+ myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
+
+ if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
+ parse_msg( id, savebuf, pos, att );
+ return id;
+ }
+
+ cout << "Check sum failure!" << endl;
+ return -1;
+}
+
+
+// load the next message of a real time data stream
+int MIDGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
+ MIDGpos *pos, MIDGatt *att )
+{
+ char tmpbuf[256];
+ char savebuf[256];
+
+ cout << "in next_message()" << endl;
+
+ bool myeof = false;
+
+ // scan for sync characters
+ uint8_t sync0, sync1;
+ serial_read( serial, tmpbuf, 2 );
+ sync0 = (unsigned char)tmpbuf[0];
+ sync1 = (unsigned char)tmpbuf[1];
+ while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
+ sync0 = sync1;
+ serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
+ cout << "scanning for start of message "
+ << (unsigned int)sync0 << " " << (unsigned int)sync1
+ << endl;
+ }
+
+ cout << "found start of message ..." << endl;
+
+ // read message id and size
+ serial_read( serial, tmpbuf, 2 );
+ uint8_t id = (unsigned char)tmpbuf[0];
+ uint8_t size = (unsigned char)tmpbuf[1];
+ // cout << "message = " << (int)id << " size = " << (int)size << endl;
+
+ // load message
+ serial_read( serial, savebuf, size );
+
+ // read checksum
+ serial_read( serial, tmpbuf, 2 );
+ uint8_t cksum0 = (unsigned char)tmpbuf[0];
+ uint8_t cksum1 = (unsigned char)tmpbuf[1];
+
+ if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
+ parse_msg( id, savebuf, pos, att );
+
+ //
+ // FIXME
+ // WRITE DATA TO LOG FILE
+ //
+
+ return id;
+ }
+
+ cout << "Check sum failure!" << endl;
+ return -1;
+
+
}
projects / flightgear.git / blobdiff