TemperatureSensor: Add Rangefinder to TEMPx_SRC sensor source options

Enable Rangefinder as a TEMPx_SRC source so a dedicated temperature
sensor (e.g. TSYS01 underwater) can override the rangefinder driver's
temperature, used by Rover/Sub WATER_DEPTH MAVLink messages.

Override is plumbed through AP_RangeFinder: a NaN-defaulted
temperature_external on RangeFinder_State, a frontend set_temperature
hook, and a backend get_temp() wrapper that returns the override when
set and otherwise falls through to the driver's _get_temp() virtual.

Pattern follows the existing convention in AP_TemperatureSensor_Backend
(TEMP_SRC_ID=1 maps to the first instance via source_id-1).

Co-authored-by: javid <namazovj@gmail.com>

Author: anishk85

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -855,15 +855,18 @@ MAV_DISTANCE_SENSOR RangeFinder::get_mav_distance_sensor_type_orient(enum Rotati
     return backend->get_mav_distance_sensor_type();
 }
 
-// get temperature reading in C.  returns true on success and populates temp argument
-bool RangeFinder::get_temp(enum Rotation orientation, float &temp) const
+#if AP_TEMPERATURE_SENSOR_ENABLED
+// set temperature from an external source
+bool RangeFinder::set_temperature(uint8_t instance, float temperature)
 {
-    AP_RangeFinder_Backend *backend = find_instance(orientation);
-    if (backend == nullptr) {
+    if (instance >= RANGEFINDER_MAX_INSTANCES) {
         return false;
     }
-    return backend->get_temp(temp);
+
+    state[instance].temperature_external = temperature;
+    return true;
 }
+#endif  // AP_TEMPERATURE_SENSOR_ENABLED
 
 #if HAL_LOGGING_ENABLED
 // Write an RFND (rangefinder) packet

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -25,6 +25,7 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_MSP/msp.h>
 #include "AP_RangeFinder_Params.h"
+#include <AP_TemperatureSensor/AP_TemperatureSensor_config.h>
 
 // Maximum number of range finder instances available on this platform
 #ifndef RANGEFINDER_MAX_INSTANCES 
@@ -228,6 +229,9 @@ class RangeFinder
         enum RangeFinder::Status status; // sensor status
         uint8_t  range_valid_count;     // number of consecutive valid readings (maxes out at 10)
         uint32_t last_reading_ms;       // system time of last successful update from sensor
+#if AP_TEMPERATURE_SENSOR_ENABLED
+        float    temperature_external = nanf("");  // NaN means no external override
+#endif
 
         const struct AP_Param::GroupInfo *var_info;
     };
@@ -314,8 +318,10 @@ class RangeFinder
     const Vector3f &get_pos_offset_orient(enum Rotation orientation) const;
     uint32_t last_reading_ms(enum Rotation orientation) const;
 
-    // get temperature reading in C.  returns true on success and populates temp argument
-    bool get_temp(enum Rotation orientation, float &temp) const;
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    // set temperature from an external source
+    bool set_temperature(uint8_t instance, float temperature);
+#endif  // AP_TEMPERATURE_SENSOR_ENABLED
 
     /*
       set an externally estimated terrain height. Used to enable power

libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp

@@ -93,4 +93,16 @@ void AP_RangeFinder_Backend::get_state(RangeFinder::RangeFinder_State &state_arg
 }
 #endif
 
+// get temperature reading in C.  returns true on success and populates temp argument
+bool AP_RangeFinder_Backend::get_temp(float &temp) const
+{
+#if AP_TEMPERATURE_SENSOR_ENABLED
+    if (!isnan(state.temperature_external)) {
+        temp = state.temperature_external;
+        return true;
+    }
+#endif
+    return _get_temp(temp);
+}
+
 #endif  // AP_RANGEFINDER_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_Backend.h

@@ -77,13 +77,15 @@ class AP_RangeFinder_Backend
     uint32_t last_reading_ms() const { return state.last_reading_ms; }
 
     // get temperature reading in C.  returns true on success and populates temp argument
-    virtual bool get_temp(float &temp) const { return false; }
+    bool get_temp(float &temp) const;
 
     // return the actual type of the rangefinder, as opposed to the
     // parameter value which may be changed at runtime.
     RangeFinder::Type allocated_type() const { return _backend_type; }
 
 protected:
+    // get temperature reading in C.  returns true on success and populates temp argument
+    virtual bool _get_temp(float &temp) const { return false; }
 
     // update status based on distance measurement
     void update_status(RangeFinder::RangeFinder_State &state_arg) const;

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@@ -54,7 +54,7 @@ bool AP_RangeFinder_NMEA::get_reading(float &reading_m)
 }
 
 // get temperature reading
-bool AP_RangeFinder_NMEA::get_temp(float &temp) const
+bool AP_RangeFinder_NMEA::_get_temp(float &temp) const
 {
     uint32_t now_ms = AP_HAL::millis();
     if ((_temp_readtime_ms == 0) || ((now_ms - _temp_readtime_ms) > read_timeout_ms())) {

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -56,7 +56,7 @@ class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend_Serial
     bool get_reading(float &reading_m) override;
 
     // get temperature reading in C.  returns true on success and populates temp argument
-    bool get_temp(float &temp) const override;
+    bool _get_temp(float &temp) const override;
 
     uint16_t read_timeout_ms() const override { return 3000; }
 

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp

@@ -21,6 +21,7 @@
 #include <AP_Logger/AP_Logger.h>
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Servo_Telem/AP_Servo_Telem.h>
+#include <AP_RangeFinder/AP_RangeFinder.h>
 
 /*
   All backends use the same parameter table and set of indices. Therefore, two
@@ -138,6 +139,16 @@ void AP_TemperatureSensor_Backend::update_external_libraries(const float tempera
             break;
 #endif // AP_SERVO_TELEM_ENABLED
 
+#if AP_RANGEFINDER_ENABLED
+        case AP_TemperatureSensor_Params::Source::Rangefinder: {
+            auto *rangefinder = AP::rangefinder();
+            if (rangefinder != nullptr) {
+                rangefinder->set_temperature(_params.source_id-1, temperature);
+            }
+            break;
+        }
+#endif
+
         case AP_TemperatureSensor_Params::Source::None:
         case AP_TemperatureSensor_Params::Source::Pitot_tube:
         default:

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp

@@ -58,13 +58,13 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = {
     // @Param: SRC
     // @DisplayName: Sensor Source
     // @Description: Sensor Source is used to designate which device's temperature report will be replaced by this temperature sensor's data. If 0 (None) then the data is only available via log. In the future a new Motor temperature report will be created for returning data directly.
-    // @Values: 0: None, 1:ESC, 2:Motor, 3:Battery Index, 4:Battery ID/SerialNumber, 5:CAN based Pitot tube, 6:DroneCAN-out on AP_Periph, 7:Servo motor, 8:Servo PCB
+    // @Values: 0: None, 1:ESC, 2:Motor, 3:Battery Index, 4:Battery ID/SerialNumber, 5:CAN based Pitot tube, 6:DroneCAN-out on AP_Periph, 7:Servo motor, 8:Servo PCB, 9:Rangefinder
     // @User: Standard
     AP_GROUPINFO("SRC", 4, AP_TemperatureSensor_Params, source, (float)Source::None),
 
     // @Param: SRC_ID
     // @DisplayName: Sensor Source Identification
-    // @Description: Sensor Source Identification is used to replace a specific instance of a system component's temperature report with the temp sensor's. Examples: TEMP_SRC = 1 (ESC), TEMP_SRC_ID = 1 will set the temp of ESC1. TEMP_SRC = 3 (BatteryIndex),TEMP_SRC_ID = 2 will set the temp of BATT2. TEMP_SRC = 4 (BatteryId/SerialNum),TEMP_SRC_ID=42 will set the temp of all batteries that have param BATTn_SERIAL = 42.
+    // @Description: Sensor Source Identification is used to replace a specific instance of a system component's temperature report with the temp sensor's. Examples: TEMP_SRC = 1 (ESC), TEMP_SRC_ID = 1 will set the temp of ESC1. TEMP_SRC = 3 (BatteryIndex),TEMP_SRC_ID = 2 will set the temp of BATT2. TEMP_SRC = 4 (BatteryId/SerialNum),TEMP_SRC_ID=42 will set the temp of all batteries that have param BATTn_SERIAL = 42. TEMP_SRC = 9 (Rangefinder),TEMP_SRC_ID = 1 will set the temp of the first rangefinder instance (RNGFND1).
     AP_GROUPINFO("SRC_ID", 5, AP_TemperatureSensor_Params, source_id, AP_TEMPERATURE_SENSOR_SOURCE_ID_DEFAULT),
 
     AP_GROUPEND

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h

@@ -50,6 +50,7 @@ class AP_TemperatureSensor_Params {
         DroneCAN                    = 6,
         Servo_Motor                 = 7,
         Servo_PCB                   = 8,
+        Rangefinder                 = 9,
     };
 
     AP_Enum<Type> type;             // 0=disabled, others see frontend enum TYPE