diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp index 68ac7027ab..69ef84e41a 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp +++ b/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 #include @@ -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: diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h index 1a4bffa656..1a9538d775 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h +++ b/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: diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp new file mode 100644 index 0000000000..0092939a06 --- /dev/null +++ b/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 . + */ + +#include "AP_TemperatureSensor_config.h" + +#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + +#include "AP_TemperatureSensor_DroneCAN.h" +#include +#include + +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 ¶ms) : + 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 + diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h new file mode 100644 index 0000000000..fa2a3f086f --- /dev/null +++ b/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 . + */ + +#pragma once + +#include "AP_TemperatureSensor_config.h" + +#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + +#include "AP_TemperatureSensor_Backend.h" +#include + +#include + +class AP_TemperatureSensor_DroneCAN : public AP_TemperatureSensor_Backend { +public: + AP_TemperatureSensor_DroneCAN(AP_TemperatureSensor &front, AP_TemperatureSensor::TemperatureSensor_State &state, AP_TemperatureSensor_Params ¶ms); + + 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 diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp index 25caea8647..b1e15333d4 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp +++ b/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), diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h index 73d60a40d5..f8c551ee5f 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h +++ b/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 diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h index b34ac029c3..8b7cbf59e4 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h +++ b/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