Add some initial support for reading live data from a serial port (and
passing it along to FlightGear.) I notice that the serial port read only seems to work correctly if I read one character at a time. Multicharacter reads seem very unreliable.
This commit is contained in:
curt
parent
ee841a149f
commit
6f9f5338c0
3 changed files with 212 additions and 118 deletions
|
|
@ -56,13 +56,17 @@ 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
|
||||
|
|
@ -365,33 +369,65 @@ 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 ) {
|
||||
bool myeof = false;
|
||||
int result = 0;
|
||||
while ( result != length && !myeof ) {
|
||||
result = ch->read( buf, length );
|
||||
if ( ch->get_type() == sgFileType ) {
|
||||
myeof = ((SGFile *)ch)->eof();
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// load the next message of a real time data stream
|
||||
int MIDGTrack::next_message( SGIOChannel *ch, 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;
|
||||
ch->read( tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
|
||||
ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
|
||||
while ( (sync0 != 129 || sync1 != 161) && !ch->eof() ) {
|
||||
myread( ch, tmpbuf, 1 ); sync0 = (unsigned char)tmpbuf[0];
|
||||
myread( ch, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
|
||||
while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
|
||||
sync0 = sync1;
|
||||
ch->read( tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
|
||||
myread( ch, 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();
|
||||
}
|
||||
}
|
||||
|
||||
// cout << "start of message ..." << endl;
|
||||
// cout << "found start of message ..." << endl;
|
||||
|
||||
// read message id and size
|
||||
ch->read( tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
|
||||
ch->read( tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
|
||||
myread( ch, tmpbuf, 1 ); uint8_t id = (unsigned char)tmpbuf[0];
|
||||
myread( ch, tmpbuf, 1 ); uint8_t size = (unsigned char)tmpbuf[0];
|
||||
// cout << "message = " << (int)id << " size = " << (int)size << endl;
|
||||
|
||||
// load message
|
||||
ch->read( savebuf, size );
|
||||
if ( ch->get_type() == sgFileType ) {
|
||||
int count = myread( ch, 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];
|
||||
}
|
||||
}
|
||||
|
||||
// read checksum
|
||||
ch->read( tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
|
||||
ch->read( tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
|
||||
myread( ch, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
|
||||
myread( ch, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
|
||||
|
||||
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
|
||||
parse_msg( id, savebuf, pos, att );
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
|
||||
#include <simgear/constants.h>
|
||||
#include <simgear/io/lowlevel.hxx> // endian tests
|
||||
#include <simgear/io/sg_serial.hxx>
|
||||
#include <simgear/math/sg_geodesy.hxx>
|
||||
#include <simgear/timing/timestamp.hxx>
|
||||
|
||||
|
|
@ -36,6 +37,7 @@ static int ctrls_port = 5506;
|
|||
|
||||
// Default path
|
||||
static string file = "";
|
||||
static string serialdev = "";
|
||||
|
||||
// Master time counter
|
||||
float sim_time = 0.0f;
|
||||
|
|
@ -322,6 +324,14 @@ int main( int argc, char **argv ) {
|
|||
usage( argv[0] );
|
||||
exit( -1 );
|
||||
}
|
||||
} else if ( strcmp( argv[i], "--serial" ) == 0 ) {
|
||||
++i;
|
||||
if ( i < argc ) {
|
||||
serialdev = argv[i];
|
||||
} else {
|
||||
usage( argv[0] );
|
||||
exit( -1 );
|
||||
}
|
||||
} else if ( strcmp( argv[i], "--host" ) == 0 ) {
|
||||
++i;
|
||||
if ( i < argc ) {
|
||||
|
|
@ -370,15 +380,6 @@ int main( int argc, char **argv ) {
|
|||
}
|
||||
}
|
||||
|
||||
// Load the track data
|
||||
if ( file == "" ) {
|
||||
cout << "No track file specified" << endl;
|
||||
exit(-1);
|
||||
}
|
||||
track.load( file );
|
||||
cout << "Loaded " << track.pos_size() << " position records." << endl;
|
||||
cout << "Loaded " << track.att_size() << " attitude records." << endl;
|
||||
|
||||
// Setup up outgoing network connections
|
||||
|
||||
netInit( &argc,argv ); // We must call this before any other net stuff
|
||||
|
|
@ -418,122 +419,179 @@ int main( int argc, char **argv ) {
|
|||
}
|
||||
cout << "connected outgoing ctrls socket" << endl;
|
||||
|
||||
int size = track.pos_size();
|
||||
if ( file.length() ) {
|
||||
// Load data from a track data
|
||||
track.load( file );
|
||||
cout << "Loaded " << track.pos_size() << " position records." << endl;
|
||||
cout << "Loaded " << track.att_size() << " attitude records." << endl;
|
||||
|
||||
double current_time = track.get_pospt(0).get_seconds();
|
||||
cout << "Track begin time is " << current_time << endl;
|
||||
double end_time = track.get_pospt(size-1).get_seconds();
|
||||
cout << "Track end time is " << end_time << endl;
|
||||
cout << "Duration = " << end_time - current_time << endl;
|
||||
int size = track.pos_size();
|
||||
|
||||
// advance skip seconds forward
|
||||
current_time += skip;
|
||||
double current_time = track.get_pospt(0).get_seconds();
|
||||
cout << "Track begin time is " << current_time << endl;
|
||||
double end_time = track.get_pospt(size-1).get_seconds();
|
||||
cout << "Track end time is " << end_time << endl;
|
||||
cout << "Duration = " << end_time - current_time << endl;
|
||||
|
||||
frame_us = 1000000.0 / hertz;
|
||||
if ( frame_us < 0.0 ) {
|
||||
frame_us = 0.0;
|
||||
}
|
||||
// advance skip seconds forward
|
||||
current_time += skip;
|
||||
|
||||
SGTimeStamp start_time;
|
||||
start_time.stamp();
|
||||
int pos_count = 0;
|
||||
int att_count = 0;
|
||||
frame_us = 1000000.0 / hertz;
|
||||
if ( frame_us < 0.0 ) {
|
||||
frame_us = 0.0;
|
||||
}
|
||||
|
||||
MIDGpos pos0, pos1;
|
||||
pos0 = pos1 = track.get_pospt( 0 );
|
||||
SGTimeStamp start_time;
|
||||
start_time.stamp();
|
||||
int pos_count = 0;
|
||||
int att_count = 0;
|
||||
|
||||
MIDGpos pos0, pos1;
|
||||
pos0 = pos1 = track.get_pospt( 0 );
|
||||
|
||||
MIDGatt att0, att1;
|
||||
att0 = att1 = track.get_attpt( 0 );
|
||||
MIDGatt att0, att1;
|
||||
att0 = att1 = track.get_attpt( 0 );
|
||||
|
||||
while ( current_time < end_time ) {
|
||||
// cout << "current_time = " << current_time << " end_time = "
|
||||
// << end_time << endl;
|
||||
while ( current_time < end_time ) {
|
||||
// cout << "current_time = " << current_time << " end_time = "
|
||||
// << end_time << endl;
|
||||
|
||||
// Advance position pointer
|
||||
while ( current_time > pos1.get_seconds()
|
||||
&& pos_count < track.pos_size() )
|
||||
{
|
||||
pos0 = pos1;
|
||||
++pos_count;
|
||||
// cout << "count = " << count << endl;
|
||||
pos1 = track.get_pospt( pos_count );
|
||||
}
|
||||
// cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time()
|
||||
// << endl;
|
||||
// Advance position pointer
|
||||
while ( current_time > pos1.get_seconds()
|
||||
&& pos_count < track.pos_size() )
|
||||
{
|
||||
pos0 = pos1;
|
||||
++pos_count;
|
||||
// cout << "count = " << count << endl;
|
||||
pos1 = track.get_pospt( pos_count );
|
||||
}
|
||||
// cout << "p0 = " << p0.get_time() << " p1 = " << p1.get_time()
|
||||
// << endl;
|
||||
|
||||
// Advance attitude pointer
|
||||
while ( current_time > att1.get_seconds()
|
||||
&& att_count < track.att_size() )
|
||||
{
|
||||
att0 = att1;
|
||||
++att_count;
|
||||
// cout << "count = " << count << endl;
|
||||
att1 = track.get_attpt( att_count );
|
||||
}
|
||||
// cout << "pos0 = " << pos0.get_seconds()
|
||||
// << " pos1 = " << pos1.get_seconds() << endl;
|
||||
// Advance attitude pointer
|
||||
while ( current_time > att1.get_seconds()
|
||||
&& att_count < track.att_size() )
|
||||
{
|
||||
att0 = att1;
|
||||
++att_count;
|
||||
// cout << "count = " << count << endl;
|
||||
att1 = track.get_attpt( att_count );
|
||||
}
|
||||
// cout << "pos0 = " << pos0.get_seconds()
|
||||
// << " pos1 = " << pos1.get_seconds() << endl;
|
||||
|
||||
double pos_percent;
|
||||
if ( fabs(pos1.get_seconds() - pos0.get_seconds()) < 0.00001 ) {
|
||||
pos_percent = 0.0;
|
||||
} else {
|
||||
pos_percent =
|
||||
(current_time - pos0.get_seconds()) /
|
||||
(pos1.get_seconds() - pos0.get_seconds());
|
||||
}
|
||||
// cout << "Percent = " << percent << endl;
|
||||
double att_percent;
|
||||
if ( fabs(att1.get_seconds() - att0.get_seconds()) < 0.00001 ) {
|
||||
att_percent = 0.0;
|
||||
} else {
|
||||
att_percent =
|
||||
(current_time - att0.get_seconds()) /
|
||||
(att1.get_seconds() - att0.get_seconds());
|
||||
}
|
||||
// cout << "Percent = " << percent << endl;
|
||||
double pos_percent;
|
||||
if ( fabs(pos1.get_seconds() - pos0.get_seconds()) < 0.00001 ) {
|
||||
pos_percent = 0.0;
|
||||
} else {
|
||||
pos_percent =
|
||||
(current_time - pos0.get_seconds()) /
|
||||
(pos1.get_seconds() - pos0.get_seconds());
|
||||
}
|
||||
// cout << "Percent = " << percent << endl;
|
||||
double att_percent;
|
||||
if ( fabs(att1.get_seconds() - att0.get_seconds()) < 0.00001 ) {
|
||||
att_percent = 0.0;
|
||||
} else {
|
||||
att_percent =
|
||||
(current_time - att0.get_seconds()) /
|
||||
(att1.get_seconds() - att0.get_seconds());
|
||||
}
|
||||
// cout << "Percent = " << percent << endl;
|
||||
|
||||
MIDGpos pos = MIDGInterpPos( pos0, pos1, pos_percent );
|
||||
MIDGatt att = MIDGInterpAtt( att0, att1, att_percent );
|
||||
// cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg
|
||||
// << endl;
|
||||
// cout << current_time << " " << p1.lat_deg << ", " << p1.lon_deg
|
||||
// << endl;
|
||||
// cout << (double)current_time << " " << pos.lat_deg << ", "
|
||||
// << pos.lon_deg << " " << att.yaw_deg << endl;
|
||||
printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
|
||||
current_time,
|
||||
pos.lat_deg, pos.lon_deg, pos.altitude_msl,
|
||||
att.yaw_rad * 180.0 / SG_PI,
|
||||
att.pitch_rad * 180.0 / SG_PI,
|
||||
att.roll_rad * 180.0 / SG_PI );
|
||||
MIDGpos pos = MIDGInterpPos( pos0, pos1, pos_percent );
|
||||
MIDGatt att = MIDGInterpAtt( att0, att1, att_percent );
|
||||
// cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg
|
||||
// << endl;
|
||||
// cout << current_time << " " << p1.lat_deg << ", " << p1.lon_deg
|
||||
// << endl;
|
||||
// cout << (double)current_time << " " << pos.lat_deg << ", "
|
||||
// << pos.lon_deg << " " << att.yaw_deg << endl;
|
||||
printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
|
||||
current_time,
|
||||
pos.lat_deg, pos.lon_deg, pos.altitude_msl,
|
||||
att.yaw_rad * 180.0 / SG_PI,
|
||||
att.pitch_rad * 180.0 / SG_PI,
|
||||
att.roll_rad * 180.0 / SG_PI );
|
||||
|
||||
send_data( pos, att );
|
||||
send_data( pos, att );
|
||||
|
||||
// Update the elapsed time.
|
||||
static bool first_time = true;
|
||||
if ( first_time ) {
|
||||
last_time_stamp.stamp();
|
||||
first_time = false;
|
||||
}
|
||||
// Update the elapsed time.
|
||||
static bool first_time = true;
|
||||
if ( first_time ) {
|
||||
last_time_stamp.stamp();
|
||||
first_time = false;
|
||||
}
|
||||
|
||||
current_time_stamp.stamp();
|
||||
/* Convert to ms */
|
||||
double elapsed_us = current_time_stamp - last_time_stamp;
|
||||
if ( elapsed_us < (frame_us - 2000) ) {
|
||||
double requested_us = (frame_us - elapsed_us) - 2000 ;
|
||||
ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ;
|
||||
}
|
||||
current_time_stamp.stamp();
|
||||
while ( current_time_stamp - last_time_stamp < frame_us ) {
|
||||
current_time_stamp.stamp();
|
||||
/* Convert to ms */
|
||||
double elapsed_us = current_time_stamp - last_time_stamp;
|
||||
if ( elapsed_us < (frame_us - 2000) ) {
|
||||
double requested_us = (frame_us - elapsed_us) - 2000 ;
|
||||
ulMilliSecondSleep ( (int)(requested_us / 1000.0) ) ;
|
||||
}
|
||||
current_time_stamp.stamp();
|
||||
while ( current_time_stamp - last_time_stamp < frame_us ) {
|
||||
current_time_stamp.stamp();
|
||||
}
|
||||
|
||||
current_time += (frame_us / 1000000.0);
|
||||
last_time_stamp = current_time_stamp;
|
||||
}
|
||||
|
||||
current_time += (frame_us / 1000000.0);
|
||||
last_time_stamp = current_time_stamp;
|
||||
}
|
||||
cout << "Processed " << pos_count << " entries in "
|
||||
<< (current_time_stamp - start_time) / 1000000 << " seconds."
|
||||
<< endl;
|
||||
} else if ( serialdev.length() ) {
|
||||
// process incoming data from the serial port
|
||||
|
||||
cout << "Processed " << pos_count << " entries in "
|
||||
<< (current_time_stamp - start_time) / 1000000 << " seconds." << endl;
|
||||
int count = 0;
|
||||
double current_time = 0.0;
|
||||
|
||||
MIDGpos pos;
|
||||
MIDGatt att;
|
||||
|
||||
uint32_t pos_time = 1;
|
||||
uint32_t att_time = 1;
|
||||
|
||||
// open the file
|
||||
SGSerial input( serialdev, "115200" );
|
||||
if ( !input.open( SG_IO_IN ) ) {
|
||||
cout << "Cannot open file: " << file << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
while ( ! input.eof() ) {
|
||||
// cout << "looking for next message ..." << endl;
|
||||
int id = track.next_message( &input, &pos, &att );
|
||||
count++;
|
||||
|
||||
if ( id == 10 ) {
|
||||
if ( att.get_msec() > att_time ) {
|
||||
att_time = att.get_msec();
|
||||
current_time = att_time;
|
||||
} else {
|
||||
cout << "oops att back in time" << endl;
|
||||
}
|
||||
} else if ( id == 12 ) {
|
||||
if ( pos.get_msec() > pos_time ) {
|
||||
pos_time = pos.get_msec();
|
||||
current_time = pos_time;
|
||||
} else {
|
||||
cout << "oops pos back in time" << endl;
|
||||
}
|
||||
}
|
||||
|
||||
printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
|
||||
current_time,
|
||||
pos.lat_deg, pos.lon_deg, pos.altitude_msl,
|
||||
att.yaw_rad * 180.0 / SG_PI,
|
||||
att.pitch_rad * 180.0 / SG_PI,
|
||||
att.roll_rad * 180.0 / SG_PI );
|
||||
|
||||
send_data( pos, att );
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ MIDGsmooth_SOURCES = \
|
|||
MIDG_main.cxx
|
||||
|
||||
MIDGsmooth_LDADD = \
|
||||
-lsgio -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \
|
||||
-lsgio -lsgserial -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \
|
||||
-lplibnet -lplibul \
|
||||
$(joystick_LIBS) $(network_LIBS) $(base_LIBS) -lz
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue