fly/flightgear
1
0
Fork 0
Code Activity

Updates to parse health packets.

Browse source
This commit is contained in:
curt 2006-11-22 04:16:07 +00:00
parent 5d1c390194
commit 3f9013ce51
3 changed files with 110 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
utils/GPSsmooth/UGear.cxx
View file

@ -83,7 +83,8 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
void UGEARTrack::parse_msg( const int id, char *buf,
struct gps *gpspacket, imu *imupacket,
nav *navpacket, servo *servopacket )
nav *navpacket, servo *servopacket,
health *healthpacket )
{
if ( id == GPS_PACKET ) {
*gpspacket = *(struct gps *)buf;
@ -125,6 +126,9 @@ void UGEARTrack::parse_msg( const int id, char *buf,
*servopacket = *(struct servo *)buf;
servopacket->time = sg_swap_double( (uint8_t *)buf, 20 );
// printf("servo time = %.3f\n", servopacket->time);
} else if ( id == HEALTH_PACKET ) {
*healthpacket = *(struct health *)buf;
healthpacket->time = sg_swap_double( (uint8_t *)buf, 12 );
} else {
cout << "unknown id = " << id << endl;
}
@ -139,16 +143,19 @@ bool UGEARTrack::load( const string &file ) {
imu imupacket;
nav navpacket;
servo servopacket;
health healthpacket;
double gps_time = 0;
double imu_time = 0;
double nav_time = 0;
double servo_time = 0;
double health_time = 0;
gps_data.clear();
imu_data.clear();
nav_data.clear();
servo_data.clear();
health_data.clear();
// open the file
SGFile input( file );
@ -160,7 +167,7 @@ bool UGEARTrack::load( const string &file ) {
while ( ! input.eof() ) {
// cout << "looking for next message ..." << endl;
int id = next_message( &input, NULL, &gpspacket, &imupacket,
&navpacket, &servopacket );
&navpacket, &servopacket, &healthpacket );
count++;
if ( id == GPS_PACKET ) {
@ -168,28 +175,35 @@ bool UGEARTrack::load( const string &file ) {
gps_data.push_back( gpspacket );
gps_time = gpspacket.time;
} else {
cout << "oops att back in time" << endl;
cout << "oops gps back in time" << endl;
}
} else if ( id == IMU_PACKET ) {
if ( imupacket.time > imu_time ) {
imu_data.push_back( imupacket );
imu_time = imupacket.time;
} else {
cout << "oops pos back in time" << endl;
cout << "oops imu back in time" << endl;
}
} else if ( id == NAV_PACKET ) {
if ( navpacket.time > nav_time ) {
nav_data.push_back( navpacket );
nav_time = navpacket.time;
} else {
cout << "oops pos back in time" << endl;
cout << "oops nav back in time" << endl;
}
} else if ( id == SERVO_PACKET ) {
if ( servopacket.time > servo_time ) {
servo_data.push_back( servopacket );
servo_time = servopacket.time;
} else {
cout << "oops pos back in time" << endl;
cout << "oops servo back in time" << endl;
}
} else if ( id == HEALTH_PACKET ) {
if ( healthpacket.time > health_time ) {
health_data.push_back( healthpacket );
health_time = healthpacket.time;
} else {
cout << "oops health back in time" << endl;
}
}
}
@ -244,7 +258,7 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log,
// load the next message of a real time data stream
int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket )
servo *servopacket, health *healthpacket )
{
char tmpbuf[256];
char savebuf[256];
@ -296,7 +310,8 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
healthpacket );
return id;
}
@ -308,7 +323,7 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log,
// load the next message of a real time data stream
int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket )
servo *servopacket, health *healthpacket )
{
char tmpbuf[256];
char savebuf[256];
@ -350,7 +365,8 @@ int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
// << endl;
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket );
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
healthpacket );
return id;
}
@ -442,3 +458,15 @@ servo UGEARInterpSERVO( const servo A, const servo B, const double percent )
return p;
}
health UGEARInterpHEALTH( const health A, const health B, const double percent )
{
health p;
p.volts_raw = interp(A.volts_raw, B.volts_raw, percent);
p.volts = interp(A.volts, B.volts, percent);
p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
p.time = interp(A.time, B.time, percent);
return p;
}

29
utils/GPSsmooth/UGear.hxx
View file

@ -26,7 +26,8 @@ enum ugPacketType {
GPS_PACKET = 0,
IMU_PACKET = 1,
NAV_PACKET = 2,
SERVO_PACKET = 3
SERVO_PACKET = 3,
HEALTH_PACKET = 4
};
struct imu {
@ -62,6 +63,12 @@ struct servo {
double time;
};
struct health {
float volts_raw; /* raw volt reading */
float volts; /* filtered volts */
uint16_t est_seconds; /* estimated useful seconds remaining */
double time;
};
// Manage a saved ugear log (track file)
class UGEARTrack {
@ -72,12 +79,13 @@ private:
vector <imu> imu_data;
vector <nav> nav_data;
vector <servo> servo_data;
vector <health> health_data;
// parse message and put current data into vector if message has a
// newer time stamp than existing data.
void parse_msg( const int id, char *buf,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket );
servo *servopacket, health *healthpacket );
public:
@ -88,10 +96,10 @@ public:
// returns id # if a valid message found.
int next_message( SGIOChannel *ch, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket );
servo *servopacket, health * healthpacket );
int next_message( SGSerialPort *serial, SGIOChannel *log,
gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket );
servo *servopacket, health *healthpacket );
// load the named file into internal buffers
bool load( const string &file );
@ -100,6 +108,7 @@ public:
inline int imu_size() const { return imu_data.size(); }
inline int nav_size() const { return nav_data.size(); }
inline int servo_size() const { return servo_data.size(); }
inline int health_size() const { return health_data.size(); }
inline gps get_gpspt( const unsigned int i )
{
@ -133,7 +142,15 @@ public:
return servo();
}
}
inline health get_healthpt( const unsigned int i )
{
if ( i < health_data.size() ) {
return health_data[i];
} else {
return health();
}
}
};
@ -142,6 +159,8 @@ gps UGEARInterpGPS( const gps A, const gps B, const double percent );
imu UGEARInterpIMU( const imu A, const imu B, const double percent );
nav UGEARInterpNAV( const nav A, const nav B, const double percent );
servo UGEARInterpSERVO( const servo A, const servo B, const double percent );
health UGEARInterpHEALTH( const health A, const health B,
const double percent );
#endif // _FG_UGEAR_II_HXX

54
utils/GPSsmooth/UGear_main.cxx
View file

@ -105,7 +105,7 @@ static void htonf (float &x)
static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket,
servo *servopacket, health *healthpacket,
FGNetFDM *fdm, FGNetCtrls *ctrls )
{
unsigned int i;
@ -278,7 +278,7 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
static void send_data( gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket ) {
servo *servopacket, health *healthpacket ) {
int len;
int fdmsize = sizeof( FGNetFDM );
@ -287,7 +287,8 @@ static void send_data( gps *gpspacket, imu *imupacket, nav *navpacket,
FGNetFDM fgfdm;
FGNetCtrls fgctrls;
ugear2fg( gpspacket, imupacket, navpacket, servopacket, &fgfdm, &fgctrls );
ugear2fg( gpspacket, imupacket, navpacket, servopacket, healthpacket,
&fgfdm, &fgctrls );
len = fdm_sock.send(&fgfdm, fdmsize, 0);
}
@ -444,6 +445,7 @@ int main( int argc, char **argv ) {
cout << "Loaded " << track.imu_size() << " imu records." << endl;
cout << "Loaded " << track.nav_size() << " nav records." << endl;
cout << "Loaded " << track.servo_size() << " servo records." << endl;
cout << "Loaded " << track.health_size() << " health records." << endl;
int size = track.imu_size();
@ -467,6 +469,7 @@ int main( int argc, char **argv ) {
int imu_count = 0;
int nav_count = 0;
int servo_count = 0;
int health_count = 0;
gps gps0, gps1;
gps0 = gps1 = track.get_gpspt( 0 );
@ -480,6 +483,9 @@ int main( int argc, char **argv ) {
servo servo0, servo1;
servo0 = servo1 = track.get_servopt( 0 );
health health0, health1;
health0 = health1 = track.get_healthpt( 0 );
while ( current_time < end_time ) {
// cout << "current_time = " << current_time << " end_time = "
// << end_time << endl;
@ -532,6 +538,18 @@ int main( int argc, char **argv ) {
// cout << "pos0 = " << pos0.get_seconds()
// << " pos1 = " << pos1.get_seconds() << endl;
// Advance health pointer
while ( current_time > health1.time
&& health_count < track.health_size() )
{
health0 = health1;
++health_count;
// cout << "count = " << count << endl;
health1 = track.get_healthpt( health_count );
}
// cout << "pos0 = " << pos0.get_seconds()
// << " pos1 = " << pos1.get_seconds() << endl;
double gps_percent;
if ( fabs(gps1.time - gps0.time) < 0.00001 ) {
gps_percent = 0.0;
@ -572,11 +590,23 @@ int main( int argc, char **argv ) {
}
// cout << "Percent = " << percent << endl;
double health_percent;
if ( fabs(health1.time - health0.time) < 0.00001 ) {
health_percent = 0.0;
} else {
health_percent =
(current_time - health0.time) /
(health1.time - health0.time);
}
// cout << "Percent = " << percent << endl;
gps gpspacket = UGEARInterpGPS( gps0, gps1, gps_percent );
imu imupacket = UGEARInterpIMU( imu0, imu1, imu_percent );
nav navpacket = UGEARInterpNAV( nav0, nav1, nav_percent );
servo servopacket = UGEARInterpSERVO( servo0, servo1,
servo_percent );
health healthpacket = UGEARInterpHEALTH( health0, health1,
health_percent );
// cout << current_time << " " << p0.lat_deg << ", " << p0.lon_deg
// << endl;
@ -591,7 +621,8 @@ int main( int argc, char **argv ) {
imupacket.psi, imupacket.the, imupacket.phi );
}
send_data( &gpspacket, &imupacket, &navpacket, &servopacket );
send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
&healthpacket );
// Update the elapsed time.
static bool first_time = true;
@ -629,11 +660,13 @@ int main( int argc, char **argv ) {
imu imupacket;
nav navpacket;
servo servopacket;
health healthpacket;
double gps_time = 0;
double imu_time = 0;
double nav_time = 0;
double servo_time = 0;
double health_time = 0;
// open the serial port device
SGSerialPort input( serialdev, 115200 );
@ -657,7 +690,8 @@ int main( int argc, char **argv ) {
while ( input.is_enabled() ) {
// cout << "looking for next message ..." << endl;
int id = track.next_message( &input, &output, &gpspacket,
&imupacket, &navpacket, &servopacket );
&imupacket, &navpacket, &servopacket,
&healthpacket );
cout << "message id = " << id << endl;
count++;
@ -689,6 +723,13 @@ int main( int argc, char **argv ) {
} else {
cout << "oops servo back in time" << endl;
}
} else if ( id == HEALTH_PACKET ) {
if ( healthpacket.time > health_time ) {
health_time = healthpacket.time;
current_time = health_time;
} else {
cout << "oops health back in time" << endl;
}
}
// if ( gpspacket.lat > -500 ) {
@ -700,7 +741,8 @@ int main( int argc, char **argv ) {
imupacket.psi*SGD_RADIANS_TO_DEGREES );
// }
send_data( &gpspacket, &imupacket, &navpacket, &servopacket );
send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
&healthpacket );
}
}