From c91ba19e7f8175e3e5a58d40f0604c7b6d18a526 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 3 May 2018 12:06:53 +1000
Subject: [PATCH] Sub: override sending of scaled_pressure3

Without this change we run the risk of sending out the same message with
vastly different data in it
---
 ArduSub/GCS_Mavlink.cpp | 12 +++---------
 ArduSub/GCS_Mavlink.h   |  3 +++
 ArduSub/Sub.h           |  1 -
 3 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index bc798ebb6d..4f81e86cd1 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -351,9 +351,9 @@ 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)
+void GCS_MAVLINK_Sub::send_scaled_pressure3()
 {
-    if (!celsius.healthy()) {
+    if (!sub.celsius.healthy()) {
         return;
     }
     mavlink_msg_scaled_pressure3_send(
@@ -361,7 +361,7 @@ void NOINLINE Sub::send_temperature(mavlink_channel_t chan)
         AP_HAL::millis(),
         0,
         0,
-        celsius.temperature() * 100);
+        sub.celsius.temperature() * 100);
 }
 
 bool GCS_MAVLINK_Sub::send_info()
@@ -519,12 +519,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
         sub.send_vfr_hud(chan);
         break;
 
-    case MSG_RAW_IMU2:
-        CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
-        send_scaled_pressure();
-        sub.send_temperature(chan);
-        break;
-
     case MSG_RPM:
 #if RPM_ENABLED == ENABLED
         CHECK_PAYLOAD_SIZE(RPM);
diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
index 7a67e31ff2..d872330919 100644
--- a/ArduSub/GCS_Mavlink.h
+++ b/ArduSub/GCS_Mavlink.h
@@ -27,6 +27,9 @@ protected:
     MAV_RESULT _handle_command_preflight_calibration_baro() override;
     MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override;
 
+    // override sending of scaled_pressure3 to send on-board temperature:
+    void send_scaled_pressure3() override;
+
 private:
 
     void handleMessage(mavlink_message_t * msg) override;
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index f08b448658..522c7a1a0b 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -486,7 +486,6 @@ 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_data_stream_send(void);
     void gcs_check_input(void);