AP_TemperatureSensor: add DroneCAN backend

libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp

@@ -29,6 +29,7 @@
 #include "AP_TemperatureSensor_MCP9600.h"
 #include "AP_TemperatureSensor_MAX31865.h"
 #include "AP_TemperatureSensor_Analog.h"
+#include "AP_TemperatureSensor_DroneCAN.h"
 
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
@@ -195,6 +196,11 @@ void AP_TemperatureSensor::init()
             case AP_TemperatureSensor_Params::Type::ANALOG:
                 drivers[instance] = new AP_TemperatureSensor_Analog(*this, _state[instance], _params[instance]);
                 break;
+#endif
+#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
+            case AP_TemperatureSensor_Params::Type::DRONECAN:
+                drivers[instance] = new AP_TemperatureSensor_DroneCAN(*this, _state[instance], _params[instance]);
+                break;
 #endif
             case AP_TemperatureSensor_Params::Type::NONE:
             default:

libraries/AP_TemperatureSensor/AP_TemperatureSensor.h

@@ -36,6 +36,7 @@ class AP_TemperatureSensor
     friend class AP_TemperatureSensor_MAX31865;
     friend class AP_TemperatureSensor_TSYS03;
     friend class AP_TemperatureSensor_Analog;
+    friend class AP_TemperatureSensor_DroneCAN;
 
 public:
 

libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp

@@ -0,0 +1,80 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_TemperatureSensor_config.h"
+
+#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
+
+#include "AP_TemperatureSensor_DroneCAN.h"
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_Math/AP_Math.h>
+
+AP_TemperatureSensor_DroneCAN* AP_TemperatureSensor_DroneCAN::_drivers[];
+uint8_t AP_TemperatureSensor_DroneCAN::_driver_instance;
+HAL_Semaphore AP_TemperatureSensor_DroneCAN::_driver_sem;
+
+extern const AP_HAL::HAL &hal;
+
+const AP_Param::GroupInfo AP_TemperatureSensor_DroneCAN::var_info[] = {
+
+    // @Param: MSG_ID
+    // @DisplayName: Temperature sensor DroneCAN message ID
+    // @Description: Sets the message device ID this backend listens for
+    // @Range: 0 65535
+    AP_GROUPINFO("MSG_ID", 1, AP_TemperatureSensor_DroneCAN, _ID, 0),
+
+    AP_GROUPEND
+};
+
+AP_TemperatureSensor_DroneCAN::AP_TemperatureSensor_DroneCAN(AP_TemperatureSensor &front,
+                                                             AP_TemperatureSensor::TemperatureSensor_State &state,
+                                                             AP_TemperatureSensor_Params &params) :
+    AP_TemperatureSensor_Backend(front, state, params)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    _state.var_info = var_info;
+
+    // Register self in static driver list
+    WITH_SEMAPHORE(_driver_sem);
+    _drivers[_driver_instance] = this;
+    _driver_instance++;
+}
+
+// Subscript to incoming temperature messages
+void AP_TemperatureSensor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+{
+    if (ap_dronecan == nullptr) {
+        return;
+    }
+
+    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, ap_dronecan->get_driver_index()) == nullptr) {
+        AP_BoardConfig::allocation_error("temp_sub");
+    }
+}
+
+void AP_TemperatureSensor_DroneCAN::handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_device_Temperature &msg)
+{
+    WITH_SEMAPHORE(_driver_sem);
+
+    for (uint8_t i = 0; i < _driver_instance; i++) {
+        if ((_drivers[i] != nullptr) && (_drivers[i]->_ID.get() == msg.device_id)) {
+            // Driver loaded and looking for this ID, set temp
+            _drivers[i]->set_temperature(KELVIN_TO_C(msg.temperature));
+        }
+    }
+}
+
+#endif // AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
+

libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h

@@ -0,0 +1,52 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "AP_TemperatureSensor_config.h"
+
+#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
+
+#include "AP_TemperatureSensor_Backend.h"
+#include <AP_Param/AP_Param.h>
+
+#include <AP_DroneCAN/AP_DroneCAN.h>
+
+class AP_TemperatureSensor_DroneCAN : public AP_TemperatureSensor_Backend {
+public:
+    AP_TemperatureSensor_DroneCAN(AP_TemperatureSensor &front, AP_TemperatureSensor::TemperatureSensor_State &state, AP_TemperatureSensor_Params &params);
+
+    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+
+    // Don't do anything in update, but still need to override the pure virtual method.
+    void update(void) override {};
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    static void handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_device_Temperature &msg);
+
+    // Static list of drivers
+    static AP_TemperatureSensor_DroneCAN *_drivers[AP_TEMPERATURE_SENSOR_MAX_INSTANCES];
+    static uint8_t _driver_instance;
+    static HAL_Semaphore _driver_sem;
+
+    // DroneCAN temperature ID to listen for
+    AP_Int32 _ID; 
+
+};
+
+#endif // AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp

@@ -34,7 +34,7 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Temperature Sensor Type
     // @Description: Enables temperature sensors
-    // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600, 3:MAX31865, 4:TSYS03, 5:Analog
+    // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600, 3:MAX31865, 4:TSYS03, 5:Analog, 6:DroneCAN
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_TemperatureSensor_Params, type, (float)Type::NONE, AP_PARAM_FLAG_ENABLE),

libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h

@@ -33,6 +33,7 @@ public:
         MAX31865                    = 3,
         TSYS03                      = 4,
         ANALOG                      = 5,
+        DRONECAN                    = 6,
     };
 
     // option to map to another system component

libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h

@@ -24,6 +24,12 @@
     #define AP_TEMPERATURE_SENSOR_ANALOG_ENABLED AP_TEMPERATURE_SENSOR_ENABLED
 #endif
 
+#ifndef AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
+    #define AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED AP_TEMPERATURE_SENSOR_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS
+#endif
+#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED && !HAL_ENABLE_DRONECAN_DRIVERS
+    #error AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED requires HAL_ENABLE_DRONECAN_DRIVERS
+#endif
 
 // maximum number of Temperature Sensors
 #ifndef AP_TEMPERATURE_SENSOR_MAX_INSTANCES