fly/flightgear
1
0
Fork 0
Code Activity

Latest round of changes ... now works.

Browse source
This commit is contained in:
curt 2006-11-13 03:57:32 +00:00
parent 16aee6ac41
commit 06dc1d6afa
2 changed files with 30 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

39
utils/GPSsmooth/UGear.cxx
View file

@ -69,9 +69,9 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
// << " [" << (unsigned int)buf[i] << "]" << endl; // << " [" << (unsigned int)buf[i] << "]" << endl;
} }
// cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0 cout << "c0 = " << (unsigned int)c0 << " (" << (unsigned int)cksum0
// << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1 << ") c1 = " << (unsigned int)c1 << " (" << (unsigned int)cksum1
// << ")" << endl; << ")" << endl;
if ( c0 == cksum0 && c1 == cksum1 ) { if ( c0 == cksum0 && c1 == cksum1 ) {
return true; return true;
@ -87,8 +87,8 @@ void UGEARTrack::parse_msg( const int id, char *buf,
{ {
if ( id == GPS_PACKET ) { if ( id == GPS_PACKET ) {
*gpspacket = *(struct gps *)buf; *gpspacket = *(struct gps *)buf;
gpspacket->lon = sg_swap_double( (uint8_t *)buf, 0 ); gpspacket->lat = sg_swap_double( (uint8_t *)buf, 0 );
gpspacket->lat = sg_swap_double( (uint8_t *)buf, 8 ); gpspacket->lon = sg_swap_double( (uint8_t *)buf, 8 );
gpspacket->alt = sg_swap_double( (uint8_t *)buf, 16 ); gpspacket->alt = sg_swap_double( (uint8_t *)buf, 16 );
gpspacket->vn = sg_swap_double( (uint8_t *)buf, 24 ); gpspacket->vn = sg_swap_double( (uint8_t *)buf, 24 );
gpspacket->ve = sg_swap_double( (uint8_t *)buf, 32 ); gpspacket->ve = sg_swap_double( (uint8_t *)buf, 32 );
@ -114,6 +114,13 @@ void UGEARTrack::parse_msg( const int id, char *buf,
// printf("imu.time = %.4f\n", imupacket->time); // printf("imu.time = %.4f\n", imupacket->time);
} else if ( id == NAV_PACKET ) { } else if ( id == NAV_PACKET ) {
*navpacket = *(struct nav *)buf; *navpacket = *(struct nav *)buf;
navpacket->lon = sg_swap_double( (uint8_t *)buf, 0 );
navpacket->lat = sg_swap_double( (uint8_t *)buf, 8 );
navpacket->alt = sg_swap_double( (uint8_t *)buf, 16 );
navpacket->vn = sg_swap_double( (uint8_t *)buf, 24 );
navpacket->ve = sg_swap_double( (uint8_t *)buf, 32 );
navpacket->vd = sg_swap_double( (uint8_t *)buf, 40 );
navpacket->time = sg_swap_double( (uint8_t *)buf, 52 );
} else if ( id == SERVO_PACKET ) { } else if ( id == SERVO_PACKET ) {
*servopacket = *(struct servo *)buf; *servopacket = *(struct servo *)buf;
servopacket->time = sg_swap_double( (uint8_t *)buf, 20 ); servopacket->time = sg_swap_double( (uint8_t *)buf, 20 );
@ -214,13 +221,18 @@ int myread( SGIOChannel *ch, SGIOChannel *log, char *buf, int length ) {
// attempt to work around some system dependent issues. Our read can // attempt to work around some system dependent issues. Our read can
// return < data than we want. // return < data than we want.
int serial_read( SGSerialPort *serial, char *buf, int length ) { int serial_read( SGSerialPort *serial, SGIOChannel *log,
char *buf, int length )
{
int result = 0; int result = 0;
int bytes_read = 0; int bytes_read = 0;
char *tmp = buf; char *tmp = buf;
while ( bytes_read < length ) { while ( bytes_read < length ) {
result = serial->read_port( tmp, length - bytes_read ); result = serial->read_port( tmp, length - bytes_read );
if ( result > 0 && log != NULL ) {
log->write( buf, result );
}
bytes_read += result; bytes_read += result;
tmp += result; tmp += result;
// cout << " read " << bytes_read << " of " << length << endl; // cout << " read " << bytes_read << " of " << length << endl;
@ -308,12 +320,12 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
// scan for sync characters // scan for sync characters
uint8_t sync0, sync1; uint8_t sync0, sync1;
result = serial_read( serial, tmpbuf, 2 ); result = serial_read( serial, log, tmpbuf, 2 );
sync0 = (unsigned char)tmpbuf[0]; sync0 = (unsigned char)tmpbuf[0];
sync1 = (unsigned char)tmpbuf[1]; sync1 = (unsigned char)tmpbuf[1];
while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) { while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
sync0 = sync1; sync0 = sync1;
serial_read( serial, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0]; serial_read( serial, log, tmpbuf, 1 ); sync1 = (unsigned char)tmpbuf[0];
cout << "scanning for start of message " cout << "scanning for start of message "
<< (unsigned int)sync0 << " " << (unsigned int)sync1 << (unsigned int)sync0 << " " << (unsigned int)sync1
<< endl; << endl;
@ -322,16 +334,16 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
// cout << "found start of message ..." << endl; // cout << "found start of message ..." << endl;
// read message id and size // read message id and size
serial_read( serial, tmpbuf, 2 ); serial_read( serial, log, tmpbuf, 2 );
uint8_t id = (unsigned char)tmpbuf[0]; uint8_t id = (unsigned char)tmpbuf[0];
uint8_t size = (unsigned char)tmpbuf[1]; uint8_t size = (unsigned char)tmpbuf[1];
cout << "message = " << (int)id << " size = " << (int)size << endl; cout << "message = " << (int)id << " size = " << (int)size << endl;
// load message // load message
serial_read( serial, savebuf, size ); serial_read( serial, log, savebuf, size );
// read checksum // read checksum
serial_read( serial, tmpbuf, 2 ); serial_read( serial, log, tmpbuf, 2 );
uint8_t cksum0 = (unsigned char)tmpbuf[0]; uint8_t cksum0 = (unsigned char)tmpbuf[0];
uint8_t cksum1 = (unsigned char)tmpbuf[1]; uint8_t cksum1 = (unsigned char)tmpbuf[1];
// cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1 // cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
@ -340,11 +352,6 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) { if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket ); parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
//
// FIXME
// WRITE DATA TO LOG FILE
//
return id; return id;
} }

12
utils/GPSsmooth/UGear_main.cxx
View file

@ -114,8 +114,8 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
fdm->version = FG_NET_FDM_VERSION; fdm->version = FG_NET_FDM_VERSION;
// Aero parameters // Aero parameters
fdm->longitude = gpspacket->lon; fdm->longitude = gpspacket->lon * SG_DEGREES_TO_RADIANS;
fdm->latitude = gpspacket->lat; fdm->latitude = gpspacket->lat * SG_DEGREES_TO_RADIANS;
fdm->altitude = gpspacket->alt; fdm->altitude = gpspacket->alt;
fdm->agl = -9999.0; fdm->agl = -9999.0;
fdm->psi = imupacket->psi; // heading fdm->psi = imupacket->psi; // heading
@ -692,10 +692,12 @@ int main( int argc, char **argv ) {
} }
// if ( gpspacket.lat > -500 ) { // if ( gpspacket.lat > -500 ) {
printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n", printf( "%.2f %.6f %.6f %.1f %.2f %.2f %.2f\n",
current_time, current_time,
gpspacket.lat, gpspacket.lon, gpspacket.alt, navpacket.lat, navpacket.lon, navpacket.alt,
imupacket.phi, imupacket.the, imupacket.psi ); imupacket.phi*SGD_RADIANS_TO_DEGREES,
imupacket.the*SGD_RADIANS_TO_DEGREES,
imupacket.psi*SGD_RADIANS_TO_DEGREES );
// } // }
send_data( &gpspacket, &imupacket, &navpacket, &servopacket ); send_data( &gpspacket, &imupacket, &navpacket, &servopacket );