#include "MIDG-II.hxx"
-SG_USING_STD(cout);
-SG_USING_STD(endl);
-SG_USING_STD(string);
+using std::cout;
+using std::endl;
+using std::string;
// Network channels
// << endl;
// cout << (double)current_time << " " << pos.lat_deg << ", "
// << pos.lon_deg << " " << att.yaw_deg << endl;
- if ( pos.lat_deg > 50 ) {
+ if ( pos.lat_deg > -500 ) {
printf( "%.3f %.4f %.4f %.1f %.2f %.2f %.2f\n",
current_time,
pos.lat_deg, pos.lon_deg, pos.altitude_msl,
current_time_stamp.stamp();
/* Convert to ms */
- double elapsed_us = current_time_stamp - last_time_stamp;
+ double elapsed_us = (current_time_stamp - last_time_stamp).toUSecs();
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 ) {
+ while ( (current_time_stamp - last_time_stamp).toUSecs() < frame_us ) {
current_time_stamp.stamp();
}
}
cout << "Processed " << pos_count << " entries in "
- << (current_time_stamp - start_time) / 1000000 << " seconds."
+ << current_time_stamp - start_time << " seconds."
<< endl;
} else if ( serialdev.length() ) {
// process incoming data from the serial port
uint32_t att_time = 1;
// open the serial port device
- SGSerial input( serialdev, "115200" );
- if ( !input.open( SG_IO_IN ) ) {
+ SGSerialPort input( serialdev, 115200 );
+ if ( !input.is_enabled() ) {
cout << "Cannot open: " << serialdev << endl;
return false;
}
return false;
}
- while ( ! input.eof() ) {
+ while ( input.is_enabled() ) {
// cout << "looking for next message ..." << endl;
int id = track.next_message( &input, &output, &pos, &att );
+ cout << "message id = " << id << endl;
count++;
if ( id == 10 ) {
}
}
- if ( pos.lat_deg > 50 ) {
- 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 );
+ if ( pos.lat_deg > -500 ) {
+ // 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 );