From 2b5e2a00e14e94df0aaa6c2d5f987a41894dc1ab Mon Sep 17 00:00:00 2001
From: Josh Henderson <hendjoshsr71@gmail.com>
Date: Sun, 26 Sep 2021 04:19:36 -0400
Subject: [PATCH] SITL: NMEA Depthfinder add temperature simulation

---
 libraries/SITL/SIM_RF_NMEA.cpp           | 11 +++++++++++
 libraries/SITL/SIM_RF_NMEA.h             |  3 +++
 libraries/SITL/SIM_SerialRangeFinder.cpp | 19 +++++++++++++++++++
 libraries/SITL/SIM_SerialRangeFinder.h   |  5 +++++
 4 files changed, 38 insertions(+)

diff --git a/libraries/SITL/SIM_RF_NMEA.cpp b/libraries/SITL/SIM_RF_NMEA.cpp
index 5e1bae4104..6f5f1aceff 100644
--- a/libraries/SITL/SIM_RF_NMEA.cpp
+++ b/libraries/SITL/SIM_RF_NMEA.cpp
@@ -38,3 +38,14 @@ uint32_t RF_NMEA::packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t bufle
     ret += snprintf((char*)&buffer[ret], buflen-ret, "*%02X\r\n", checksum);
     return ret;
 }
+
+uint32_t RF_NMEA::packet_for_temperature(float temperature, uint8_t *buffer, uint8_t buflen)
+{
+    ssize_t ret = snprintf((char*)buffer, buflen, "$SMMTW,%f %f", temperature, 0.01);
+    uint8_t checksum = 0;
+    for (uint8_t i=1; i<ret; i++) { // 1 because the initial $ is skipped
+        checksum ^= buffer[i];
+    }
+    ret += snprintf((char*)&buffer[ret], buflen-ret, "*%02X\r\n", checksum);
+    return ret;
+}
diff --git a/libraries/SITL/SIM_RF_NMEA.h b/libraries/SITL/SIM_RF_NMEA.h
index b9967f8d9f..7a667a7fdc 100644
--- a/libraries/SITL/SIM_RF_NMEA.h
+++ b/libraries/SITL/SIM_RF_NMEA.h
@@ -38,6 +38,9 @@ public:
 
     uint32_t packet_for_alt(uint16_t alt_cm, uint8_t *buffer, uint8_t buflen) override;
 
+    bool has_temperature() const override { return true; }
+    uint32_t packet_for_temperature(float temperature, uint8_t *buffer, uint8_t buflen) override;
+
 private:
 
 };
diff --git a/libraries/SITL/SIM_SerialRangeFinder.cpp b/libraries/SITL/SIM_SerialRangeFinder.cpp
index dafa511fea..7111550d58 100644
--- a/libraries/SITL/SIM_SerialRangeFinder.cpp
+++ b/libraries/SITL/SIM_SerialRangeFinder.cpp
@@ -36,4 +36,23 @@ void SerialRangeFinder::update(float range)
                                               ARRAY_SIZE(data));
 
     write_to_autopilot((char*)data, packetlen);
+
+    if (has_temperature()) {
+        send_temperature();
+    }
+
+}
+
+void SerialRangeFinder::send_temperature()
+{
+    // Use the simple underwater model to get temperature
+    float rho, delta, theta;
+    AP_Baro::SimpleUnderWaterAtmosphere(-0.5 * 0.001, rho, delta, theta); // get simulated temperature for 0.5m depth
+    const float temperature = Aircraft::rand_normal(SSL_AIR_TEMPERATURE * theta - C_TO_KELVIN, 1); // FIXME pick a stddev based on data sheet
+
+    uint8_t data[255];
+    const uint32_t packetlen = packet_for_temperature(temperature,
+                                                        data,
+                                                        ARRAY_SIZE(data));
+    write_to_autopilot((char*)data, packetlen);
 }
diff --git a/libraries/SITL/SIM_SerialRangeFinder.h b/libraries/SITL/SIM_SerialRangeFinder.h
index e6df1dd8e1..4a4a40f98e 100644
--- a/libraries/SITL/SIM_SerialRangeFinder.h
+++ b/libraries/SITL/SIM_SerialRangeFinder.h
@@ -38,7 +38,12 @@ public:
 
     virtual uint16_t reading_interval_ms() const { return 200; } // 5Hz default
 
+    // Rangefinders that return temperature most likely a depthfinder for boats
+    virtual bool has_temperature() const { return false; }
+    virtual uint32_t packet_for_temperature(float temperature, uint8_t *buffer, uint8_t buflen) { return 0; }; // 0 length packet by default
+
 private:
+    void send_temperature();
 
     uint32_t last_sent_ms;
 };