From 37760389612e7a8f274e41b309b8f59db3299c42 Mon Sep 17 00:00:00 2001
From: Soo-Hyun Yoo <yoos117@gmail.com>
Date: Wed, 17 Jun 2015 14:26:10 -0700
Subject: [PATCH] refs #45 Downsample some data logging streams.

---
 .../atmospheric_location_estimator.cpp        |  7 +++-
 src/estimator/dcm_attitude_estimator.cpp      |  9 ++++-
 src/estimator/world_estimator.cpp             | 37 ++++++++++---------
 3 files changed, 33 insertions(+), 20 deletions(-)

diff --git a/src/estimator/atmospheric_location_estimator.cpp b/src/estimator/atmospheric_location_estimator.cpp
index 34c6cff..fd83288 100644
--- 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
index 38648bb..02619a9 100644
--- 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
index e0a94ae..1977546 100644
--- 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 {