From 6fab407d96c868b1d9ed87d65ef1c7f10c78a9d8 Mon Sep 17 00:00:00 2001
From: Soo-Hyun Yoo <yoos117@gmail.com>
Date: Mon, 9 Feb 2015 23:36:41 -0800
Subject: [PATCH] refs #17 Fix helios thermistor reading dataflow.

Still freezes after printing twice.
---
 src/drivers/ublox_neo7.cpp                |  6 +++---
 src/drivers/vishaytherm.cpp               |  3 +++
 src/estimator/world_estimator.cpp         |  2 +-
 variants/platforms/aerial_v4/platform.cpp | 11 ++++++++---
 variants/units/helios/unit_data.hpp       |  8 +++++++-
 5 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/src/drivers/ublox_neo7.cpp b/src/drivers/ublox_neo7.cpp
index 3b6ace3..461a1bb 100644
--- a/src/drivers/ublox_neo7.cpp
+++ b/src/drivers/ublox_neo7.cpp
@@ -13,9 +13,9 @@ GPSReading UBloxNEO7::readGPS() {
   read(10);   // TODO
   //chprintf((BaseSequentialStream*)&SD4, "%10c", rxbuf.data());
 
-  char dbg_buf[16];
-  chsnprintf(dbg_buf, 12, "%12c", rxbuf.data());
-  chnWriteTimeout((BaseChannel*)&SD4, (uint8_t*)dbg_buf, 12, MS2ST(20));
+  //char dbg_buf[18];
+  //chsnprintf(dbg_buf, 12, "%12c", rxbuf.data());
+  //chnWriteTimeout((BaseChannel*)&SD4, (uint8_t*)dbg_buf, 12, MS2ST(20));
 
   // Run parser
   // TODO
diff --git a/src/drivers/vishaytherm.cpp b/src/drivers/vishaytherm.cpp
index 4d1dcf3..f9f4160 100644
--- a/src/drivers/vishaytherm.cpp
+++ b/src/drivers/vishaytherm.cpp
@@ -1,5 +1,6 @@
 #include "drivers/vishaytherm.hpp"
 
+#include "chprintf.h"
 #include "unit_config.hpp"
 
 void VishayTherm::init() {
@@ -10,5 +11,7 @@ ThermistorReading VishayTherm::readTherm() {
 
   reading.celsius = 2.345f;
 
+  chprintf((BaseSequentialStream*)&SD4, "%f\r\n", reading.celsius);
+
   return reading;
 }
diff --git a/src/estimator/world_estimator.cpp b/src/estimator/world_estimator.cpp
index 549b210..a27abb5 100644
--- a/src/estimator/world_estimator.cpp
+++ b/src/estimator/world_estimator.cpp
@@ -18,7 +18,7 @@ WorldEstimate WorldEstimator::update(const SensorReadingGroup& readings) {
 
   if(worldMessageStream.ready()) {
     protocol::message::log_message_t m;
-    sprintf(m.data, "world estimate test");
+    sprintf(m.data, "therm: %f", readings.therm->celsius);
 
     worldMessageStream.publish(m);
   }
diff --git a/variants/platforms/aerial_v4/platform.cpp b/variants/platforms/aerial_v4/platform.cpp
index b51c08f..fa99330 100644
--- a/variants/platforms/aerial_v4/platform.cpp
+++ b/variants/platforms/aerial_v4/platform.cpp
@@ -105,9 +105,8 @@ VishayTherm& Platform::get() {
 }
 
 template <>
-ADCPlatform& Platform::get() {
-  static ADCPlatform adcPlatform;
-  return adcPlatform;
+Thermistor& Platform::get() {
+  return get<VishayTherm>();
 }
 
 template <>
@@ -121,6 +120,12 @@ GPS& Platform::get() {
   return get<UBloxNEO7>();
 }
 
+template <>
+ADCPlatform& Platform::get() {
+  static ADCPlatform adcPlatform;
+  return adcPlatform;
+}
+
 template <>
 I2CPlatform& Platform::get() {
   static I2CPlatform i2cPlatform;
diff --git a/variants/units/helios/unit_data.hpp b/variants/units/helios/unit_data.hpp
index f104d7e..eeafdcf 100644
--- a/variants/units/helios/unit_data.hpp
+++ b/variants/units/helios/unit_data.hpp
@@ -9,6 +9,7 @@
 #include "sensor/gyroscope.hpp"
 #include "sensor/accelerometer.hpp"
 #include "sensor/gps.hpp"
+#include "sensor/thermistor.hpp"
 #include "system/multirotor_vehicle_system.hpp"
 #include "util/optional.hpp"
 #include "variant/platform.hpp"
@@ -38,7 +39,12 @@ struct UnitData {
       world(communicator),
       attitude(communicator),
       inputSource(communicator),
-      system(platform.get<Gyroscope>(), platform.get<Accelerometer>(), platform.get<GPS>(), std::experimental::nullopt, std::experimental::nullopt, world, attitude, inputSource, motorMapper, communicator) {
+      system(platform.get<Gyroscope>(),
+             platform.get<Accelerometer>(),
+             platform.get<GPS>(),
+             std::experimental::nullopt,
+             std::experimental::make_optional(&platform.get<Thermistor>()),
+             world, attitude, inputSource, motorMapper, communicator) {
   }
 };