Sub: Add TSYS01 'celsius' object

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:

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);

ArduSub/make.inc

@@ -57,3 +57,4 @@ LIBRARIES += AP_JSButton
 LIBRARIES += AP_LeakDetector
 LIBRARIES += AP_Gripper
 LIBRARIES += AP_Beacon
+LIBRARIES += AP_TemperatureSensor

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.

ArduSub/wscript

@@ -30,6 +30,7 @@ def build(bld):
             'AP_Proximity',
             'AP_Gripper',
             'AP_Beacon',
+            'AP_TemperatureSensor'
         ],
         use='mavlink',
     )