Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_TemperatureSensor_config.h"
      17             : 
      18             : #if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
      19             : 
      20             : #include "AP_TemperatureSensor_DroneCAN.h"
      21             : #include <AP_BoardConfig/AP_BoardConfig.h>
      22             : #include <AP_Math/AP_Math.h>
      23             : 
      24             : AP_TemperatureSensor_DroneCAN* AP_TemperatureSensor_DroneCAN::_drivers[];
      25             : uint8_t AP_TemperatureSensor_DroneCAN::_driver_instance;
      26             : HAL_Semaphore AP_TemperatureSensor_DroneCAN::_driver_sem;
      27             : 
      28             : extern const AP_HAL::HAL &hal;
      29             : 
      30             : const AP_Param::GroupInfo AP_TemperatureSensor_DroneCAN::var_info[] = {
      31             : 
      32             :     // @Param: MSG_ID
      33             :     // @DisplayName: Temperature sensor DroneCAN message ID
      34             :     // @Description: Sets the message device ID this backend listens for
      35             :     // @Range: 0 65535
      36             :     AP_GROUPINFO("MSG_ID", 1, AP_TemperatureSensor_DroneCAN, _ID, 0),
      37             : 
      38             :     // CHECK/UPDATE INDEX TABLE IN AP_TemperatureSensor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS
      39             :     // MSG_ID here clashes with pin in the analog backed,
      40             :     // but because there different types (AP_Int8 vs AP_Int32) we get away with it.
      41             : 
      42             :     AP_GROUPEND
      43             : };
      44             : 
      45           0 : AP_TemperatureSensor_DroneCAN::AP_TemperatureSensor_DroneCAN(AP_TemperatureSensor &front,
      46             :                                                              AP_TemperatureSensor::TemperatureSensor_State &state,
      47           0 :                                                              AP_TemperatureSensor_Params &params) :
      48           0 :     AP_TemperatureSensor_Backend(front, state, params)
      49             : {
      50           0 :     AP_Param::setup_object_defaults(this, var_info);
      51           0 :     _state.var_info = var_info;
      52             : 
      53             :     // Register self in static driver list
      54           0 :     WITH_SEMAPHORE(_driver_sem);
      55           0 :     _drivers[_driver_instance] = this;
      56           0 :     _driver_instance++;
      57           0 : }
      58             : 
      59             : // Subscript to incoming temperature messages
      60           5 : bool AP_TemperatureSensor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
      61             : {
      62           5 :     const auto driver_index = ap_dronecan->get_driver_index();
      63             : 
      64           5 :     return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, driver_index) != nullptr);
      65             : }
      66             : 
      67           0 : void AP_TemperatureSensor_DroneCAN::handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_device_Temperature &msg)
      68             : {
      69           0 :     WITH_SEMAPHORE(_driver_sem);
      70             : 
      71           0 :     for (uint8_t i = 0; i < _driver_instance; i++) {
      72           0 :         if ((_drivers[i] != nullptr) && (_drivers[i]->_ID.get() == msg.device_id)) {
      73             :             // Driver loaded and looking for this ID, set temp
      74           0 :             _drivers[i]->set_temperature(KELVIN_TO_C(msg.temperature));
      75             :         }
      76             :     }
      77           0 : }
      78             : 
      79             : #endif // AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
      80             :