refs #45 Downsample some data logging streams.

OSURoboticsClub · Jun 17, 2015 · 3776038 · 3776038
1 parent 7c55f51
commit 3776038
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 20 deletions.
diff --git a/src/estimator/atmospheric_location_estimator.cpp b/src/estimator/atmospheric_location_estimator.cpp
@@ -106,5 +106,10 @@ void AtmosphericLocationEstimator::updateStream() {
     locationMessageStream.publish(m);
   }
 
-  logger.write(m);
+  // Downsample logging to 100 Hz
+  static int i=0;
+  if (i % 10 == 0) {
+    logger.write(m);
+  }
+  i = (i+1) % 1000;
 }
diff --git a/src/estimator/dcm_attitude_estimator.cpp b/src/estimator/dcm_attitude_estimator.cpp
@@ -174,5 +174,12 @@ void DCMAttitudeEstimator::updateStream() {
     attitudeMessageStream.publish(m);
   }
 
-  logger.write(m);
+  // Full-resolution attitude log by itself accounts for nearly half of all
+  // logging. Downsample to 100 Hz so the filesystem has an easier time keeping
+  // up.
+  static int i=0;
+  if (i % 10 == 0) {
+    logger.write(m);
+  }
+  i = (i+1) % 1000;
 }
diff --git a/src/estimator/world_estimator.cpp b/src/estimator/world_estimator.cpp
@@ -25,27 +25,28 @@ WorldEstimate WorldEstimator::update(const SensorMeasurements& meas) {
   estimate.att = attEstimator.update(meas);
   estimate.loc = locEstimator.update(meas, estimate.att);
 
+  // TODO(yoos): Need RateLimitedStream for logger.
+  static uint16_t i = 0;
   // Log 1 kHz stream
-  protocol::message::raw_1000_message_t msg_1000 {
-    .time = ST2MS(chibios_rt::System::getTime()),
-    .accel  = {(*meas.accel).axes[0],
-               (*meas.accel).axes[1],
-               (*meas.accel).axes[2]},
-    .accelH = {(*meas.accelH).axes[0],
-               (*meas.accelH).axes[1],
-               (*meas.accelH).axes[2]},
-    .gyro   = {(*meas.gyro).axes[0],
-               (*meas.gyro).axes[1],
-               (*meas.gyro).axes[2]}
-  };
-  logger.write(msg_1000);
-  if (raw1000MessageStream.ready()) {
-    raw1000MessageStream.publish(msg_1000);
+  if (i % 1 == 0) {
+    protocol::message::raw_1000_message_t msg_1000 {
+      .time = ST2MS(chibios_rt::System::getTime()),
+        .accel  = {(*meas.accel).axes[0],
+          (*meas.accel).axes[1],
+          (*meas.accel).axes[2]},
+        .accelH = {(*meas.accelH).axes[0],
+          (*meas.accelH).axes[1],
+          (*meas.accelH).axes[2]},
+        .gyro   = {(*meas.gyro).axes[0],
+          (*meas.gyro).axes[1],
+          (*meas.gyro).axes[2]}
+    };
+    logger.write(msg_1000);
+    if (raw1000MessageStream.ready()) {
+      raw1000MessageStream.publish(msg_1000);
+    }
   }
 
-  // Downsampled streams
-  // TODO(yoos): Need RateLimitedStream for logger.
-  static uint16_t i = 0;
   // Log 50 Hz stream
   if (i % 20 == 0) {
     protocol::message::raw_50_message_t msg_50 {