From 098a716d410f69d762045b085c46ecc1d4b7c479 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Fri, 13 Jan 2017 14:51:48 -0500
Subject: [PATCH] Sub: Add TSYS01 'celsius' object

---
 ArduSub/GCS_Mavlink.cpp | 13 +++++++++++++
 ArduSub/Sub.h           |  3 +++
 ArduSub/make.inc        |  1 +
 ArduSub/system.cpp      |  2 ++
 ArduSub/wscript         |  1 +
 5 files changed, 20 insertions(+)

diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index 0b55c70608..4415f3ee52 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -448,6 +448,18 @@ void NOINLINE Sub::send_rpm(mavlink_channel_t chan)
 }
 #endif
 
+// Work around to get temperature sensor data out
+void NOINLINE Sub::send_temperature(mavlink_channel_t chan) {
+	if(!celsius.healthy()) {
+		return;
+	}
+	mavlink_msg_scaled_pressure3_send(
+			chan,
+			AP_HAL::millis(),
+			0,
+			0,
+			celsius.temperature() * 100);
+}
 
 /*
   send PID tuning message
@@ -622,6 +634,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
     case MSG_RAW_IMU2:
         CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
         send_scaled_pressure(sub.barometer);
+        sub.send_temperature(chan);
         break;
 
     case MSG_RAW_IMU3:
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index fba28f0f4d..200a4738f1 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -88,6 +88,7 @@
 #include <AC_InputManager/AC_InputManager.h>        // Pilot input handling library
 #include <AP_JSButton/AP_JSButton.h>   // Joystick/gamepad button function assignment
 #include "../libraries/AP_LeakDetector/AP_LeakDetector.h" // Leak detector
+#include <AP_TemperatureSensor/TSYS01.h>
 #include "defines.h"
 #include "config.h"
 
@@ -176,6 +177,7 @@ private:
     // flight modes convenience array
     AP_Int8 *flight_modes;
 
+    TSYS01 celsius;
     AP_Baro barometer;
     Compass compass;
     AP_InertialSensor ins;
@@ -538,6 +540,7 @@ private:
     void send_rpm(mavlink_channel_t chan);
     void rpm_update();
 #endif
+    void send_temperature(mavlink_channel_t chan);
     void send_pid_tuning(mavlink_channel_t chan);
     void gcs_send_message(enum ap_message id);
     void gcs_send_mission_item_reached_message(uint16_t mission_index);
diff --git a/ArduSub/make.inc b/ArduSub/make.inc
index 4192b98e06..9d95d55eb8 100644
--- a/ArduSub/make.inc
+++ b/ArduSub/make.inc
@@ -57,3 +57,4 @@ LIBRARIES += AP_JSButton
 LIBRARIES += AP_LeakDetector
 LIBRARIES += AP_Gripper
 LIBRARIES += AP_Beacon
+LIBRARIES += AP_TemperatureSensor
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index 19c2ee6d60..720510fd20 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -124,6 +124,8 @@ void Sub::init_ardupilot()
     
     barometer.init();
 
+    celsius.init();
+
     // Register the mavlink service callback. This will run
     // anytime there are more than 5ms remaining in a call to
     // hal.scheduler->delay.
diff --git a/ArduSub/wscript b/ArduSub/wscript
index db14ab63e2..cc654dbd7a 100644
--- a/ArduSub/wscript
+++ b/ArduSub/wscript
@@ -30,6 +30,7 @@ def build(bld):
             'AP_Proximity',
             'AP_Gripper',
             'AP_Beacon',
+            'AP_TemperatureSensor'
         ],
         use='mavlink',
     )