Legacy navdata timestamp fix by @JakobEngel, addresses #39

README.md

@@ -88,7 +88,7 @@ Information received from the drone will be published to the `ardrone/navdata` t
 * `altd`: Estimated altitude (mm)
 * `vx`, `vy`, `vz`: Linear velocity (mm/s) [TBA: Convention]
 * `ax`, `ay`, `az`: Linear acceleration (g) [TBA: Convention]
-* `tm`: Timestamp of the data returned by the Drone returned as a packed uint32 (sec:11; usec:21)
+* `tm`: Timestamp of the data returned by the Drone returned as number of micro-seconds passed since Drone's bootup.
 
 ### IMU data
 

src/ardrone_driver.cpp

@@ -557,6 +557,8 @@ void ARDroneDriver::publish_video()
 void ARDroneDriver::publish_navdata()
 {
     // Thread safe copy of interesting Navdata data
+    // TODO: This is a very expensive task, can we optimize here?
+    // maybe ignoring the copy when it is not needed.
     vp_os_mutex_lock(&navdata_lock);
     navdata_raw = shared_raw_navdata;
     vp_os_mutex_unlock(&navdata_lock);
@@ -627,7 +629,9 @@ void ARDroneDriver::publish_navdata()
     msg.vx = navdata_raw.navdata_demo.vx; // mm/sec
     msg.vy = -navdata_raw.navdata_demo.vy; // mm/sec
     msg.vz = -navdata_raw.navdata_demo.vz; // mm/sec
-    msg.tm = navdata_raw.navdata_time.time;
+    //msg.tm = navdata_raw.navdata_time.time;
+    // First 21 bits (LSB) are usecs + 11 HSB are seconds
+    msg.tm = (navdata_raw.navdata_time.time & 0x001FFFFF) + (navdata_raw.navdata_time.time >> 21)*1000000;
     msg.ax = navdata_raw.navdata_phys_measures.phys_accs[ACC_X] / 1000.0; // g
     msg.ay = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Y] / 1000.0; // g
     msg.az = -navdata_raw.navdata_phys_measures.phys_accs[ACC_Z] / 1000.0; // g

src/ardrone_sdk.cpp

@@ -224,6 +224,7 @@ extern "C" {
 
     C_RESULT navdata_custom_process(const navdata_unpacked_t * const pnd) {
         vp_os_mutex_lock(&navdata_lock);
+        // TODO: This is expensive, too (1908 Bytes)!
         shared_raw_navdata = *pnd;
         if(fullspeed_navdata)
         {