fly/flightgear
1
0
Fork 0
Code Activity

Add a pass through so we can capture IMU/INS/GPS data to a file at the same

Browse source 
time as using it to drive a copy of FlightGear.
This commit is contained in:
curt 2005-09-26 21:19:01 +00:00
parent 6f9f5338c0
commit 02512e28c0
3 changed files with 53 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

30
utils/GPSsmooth/MIDG-II.cxx
View file

@ -344,7 +344,7 @@ bool MIDGTrack::load( const string &file ) {
while ( ! input.eof() ) {
// cout << "looking for next message ..." << endl;
int id = next_message( &input, &pos, &att );
int id = next_message( &input, NULL, &pos, &att );
count++;
if ( id == 10 ) {
@ -371,7 +371,7 @@ bool MIDGTrack::load( const string &file ) {
// attempt to work around some system dependent issues. Our read can
// return < data than we want.
int myread( SGIOChannel *ch, char *buf, int length ) {
int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
bool myeof = false;
int result = 0;
while ( result != length && !myeof ) {
@ -381,11 +381,17 @@ int myread( SGIOChannel *ch, char *buf, int length ) {
}
}
if ( result > 0 && log != NULL ) {
log->write( buf, result );
}
return result;
}
// load the next message of a real time data stream
int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) {
int MIDGTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
MIDGpos *pos, MIDGatt *att )
{
char tmpbuf[256];
char savebuf[256];
@ -395,11 +401,11 @@ int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) {
// scan for sync characters
uint8_t sync0, sync1;
myread( ch, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
myread( ch, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
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, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
myread( ch, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
// cout << "scanning for start of message, eof = " << ch->eof() << endl;
if ( ch->get_type() == sgFileType ) {
myeof = ((SGFile *)ch)->eof();
@ -409,25 +415,25 @@ int MIDGTrack::next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att ) {
// cout << "found start of message ..." << endl;
// read message id and size
myread( ch, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
myread( ch, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
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, savebuf, size );
int count = myread( ch, log, savebuf, size );
if ( count != size ) {
cout << "ERROR: didn't read enough bytes!" << endl;
}
} else {
for ( int i = 0; i < size; ++i ) {
myread( ch, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
myread( ch, log, tmpbuf, 1 ); savebuf[i] = tmpbuf[0];
}
}
// read checksum
myread( ch, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
myread( ch, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
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 );

3
utils/GPSsmooth/MIDG-II.hxx
View file

@ -141,7 +141,8 @@ public:
// read/parse the next message from the specified data stream,
// returns id # if a valid message found.
int next_message( SGIOChannel *ch, MIDGpos *pos, MIDGatt *att );
int next_message( SGIOChannel *ch, SGIOChannel *log,
MIDGpos *pos, MIDGatt *att );
// load the named file into internal buffers
bool load( const string &file );

42
utils/GPSsmooth/MIDG_main.cxx
View file

@ -10,6 +10,7 @@
#include <simgear/constants.h>
#include <simgear/io/lowlevel.hxx> // endian tests
#include <simgear/io/sg_file.hxx>
#include <simgear/io/sg_serial.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/timing/timestamp.hxx>
@ -36,8 +37,9 @@ static int fdm_port = 5505;
static int ctrls_port = 5506;
// Default path
static string file = "";
static string infile = "";
static string serialdev = "";
static string outfile = "";
// Master time counter
float sim_time = 0.0f;
@ -287,7 +289,9 @@ static void send_data( const MIDGpos pos, const MIDGatt att ) {
void usage( const string &argv0 ) {
cout << "Usage: " << argv0 << endl;
cout << "\t[ --help ]" << endl;
cout << "\t[ --file <file_name>" << endl;
cout << "\t[ --infile <infile_name>" << endl;
cout << "\t[ --serial <dev_name>" << endl;
cout << "\t[ --outfile <outfile_name> (capture the data to a file)" << endl;
cout << "\t[ --hertz <hertz> ]" << endl;
cout << "\t[ --host <hostname> ]" << endl;
cout << "\t[ --broadcast ]" << endl;
@ -316,10 +320,18 @@ int main( int argc, char **argv ) {
usage( argv[0] );
exit( -1 );
}
} else if ( strcmp( argv[i], "--file" ) == 0 ) {
} else if ( strcmp( argv[i], "--infile" ) == 0 ) {
++i;
if ( i < argc ) {
file = argv[i];
infile = argv[i];
} else {
usage( argv[0] );
exit( -1 );
}
} else if ( strcmp( argv[i], "--outfile" ) == 0 ) {
++i;
if ( i < argc ) {
outfile = argv[i];
} else {
usage( argv[0] );
exit( -1 );
@ -419,9 +431,9 @@ int main( int argc, char **argv ) {
}
cout << "connected outgoing ctrls socket" << endl;
if ( file.length() ) {
if ( infile.length() ) {
// Load data from a track data
track.load( file );
track.load( infile );
cout << "Loaded " << track.pos_size() << " position records." << endl;
cout << "Loaded " << track.att_size() << " attitude records." << endl;
@ -554,16 +566,28 @@ int main( int argc, char **argv ) {
uint32_t pos_time = 1;
uint32_t att_time = 1;
// open the file
// open the serial port device
SGSerial input( serialdev, "115200" );
if ( !input.open( SG_IO_IN ) ) {
cout << "Cannot open file: " << file << endl;
cout << "Cannot open: " << serialdev << endl;
return false;
}
// open up the data log file if requested
if ( !outfile.length() ) {
cout << "no --outfile <name> specified, cannot capture data!"
<< endl;
return false;
}
SGFile output( outfile );
if ( !output.open( SG_IO_OUT ) ) {
cout << "Cannot open: " << outfile << endl;
return false;
}
while ( ! input.eof() ) {
// cout << "looking for next message ..." << endl;
int id = track.next_message( &input, &pos, &att );
int id = track.next_message( &input, &output, &pos, &att );
count++;
if ( id == 10 ) {