commit 5a0a2ec117f9160de3f590e2fb0a646277529ac1
author Brian Silverman <brians> Sat Mar 08 12:44:58 2014 -0800
committer Brian Silverman <brians> Sat Mar 08 12:44:58 2014 -0800
tree 88e05febd6c49c05b665a42f24cae252b8a4f091
parent a33af74f47243bfef881b14cbe43ecb85ca226a1

scale sonar readings correctly

frc971/input/sensor_receiver.cc

diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 7d12b48..f63d13d 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -64,6 +64,10 @@
   return adc_translate(in) * kDividerBig / kDividerSmall;
 }
 
+double sonar_translate(uint32_t in) {
+  return static_cast<double>(in) / 1000.0;
+}
+
 double hall_translate(const constants::ShifterHallEffect &k, uint16_t in_low,
                       uint16_t in_high) {
   const double low_ratio =
@@ -129,9 +133,10 @@
                     State *state) {
   ::frc971::logging_structs::CapeReading reading_to_log(
       cape_timestamp.sec(), cape_timestamp.nsec(),
-      data->main.ultrasonic_pulse_length, sizeof(*data));
+      sizeof(*data), sonar_translate(data->main.ultrasonic_pulse_length));
   LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
   bool bad_gyro;
+  // TODO(brians): Switch to LogInterval for these things.
   if (data->uninitialized_gyro) {
     LOG(DEBUG, "uninitialized gyro\n");
     bad_gyro = true;
@@ -141,10 +146,6 @@
   } else if (data->bad_gyro) {
     LOG(ERROR, "bad gyro\n");
     bad_gyro = true;
-    othersensors.MakeWithBuilder()
-        .gyro_angle(0)
-        .sonar_distance(data->main.ultrasonic_pulse_length)
-        .Send();
   } else if (data->old_gyro_reading) {
     LOG(WARNING, "old/bad gyro reading\n");
     bad_gyro = true;
@@ -155,7 +156,8 @@
   if (!bad_gyro) {
     othersensors.MakeWithBuilder()
         .gyro_angle(gyro_translate(data->gyro_angle))
-        .sonar_distance(data->main.ultrasonic_pulse_length)
+        .sonar_distance(
+            sonar_translate(data->main.ultrasonic_pulse_length))
         .Send();
   }
 

frc971/queues/to_log.q

diff --git a/frc971/queues/to_log.q b/frc971/queues/to_log.q
index a8bee3f..77e87e3 100644
--- a/frc971/queues/to_log.q
+++ b/frc971/queues/to_log.q
@@ -3,6 +3,6 @@
 struct CapeReading {
   uint32_t sec;
   uint32_t nsec;
-  uint32_t sonar;
   uint64_t struct_size;
+  double sonar;
 };