- 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());
+ 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;
+ 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 );
+
+ // 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).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).toUSecs() < frame_us ) {
+ current_time_stamp.stamp();
+ }
+
+ current_time += (frame_us / 1000000.0);
+ last_time_stamp = current_time_stamp;