SITL: NMEA Depthfinder add temperature simulation

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;
+}

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:
 
 };

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

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