Line data    Source code

       1             : /*
       2             :  * This file is free software: you can redistribute it and/or modify it
       3             :  * under the terms of the GNU General Public License as published by the
       4             :  * Free Software Foundation, either version 3 of the License, or
       5             :  * (at your option) any later version.
       6             :  *
       7             :  * This file is distributed in the hope that it will be useful, but
       8             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      10             :  * See the GNU General Public License for more details.
      11             :  *
      12             :  * You should have received a copy of the GNU General Public License along
      13             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   DroneCAN support for OpenDroneID
      17             :  */
      18             : 
      19             : #include "AP_OpenDroneID.h"
      20             : 
      21             : #if AP_OPENDRONEID_ENABLED
      22             : 
      23             : #include <AP_DroneCAN/AP_DroneCAN.h>
      24             : 
      25             : #if HAL_ENABLE_DRONECAN_DRIVERS
      26             : #include <GCS_MAVLink/GCS.h>
      27             : 
      28             : static Canard::Publisher<dronecan_remoteid_Location>* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS];
      29             : static Canard::Publisher<dronecan_remoteid_BasicID>* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
      30             : static Canard::Publisher<dronecan_remoteid_SelfID>* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
      31             : static Canard::Publisher<dronecan_remoteid_System>* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS];
      32             : static Canard::Publisher<dronecan_remoteid_OperatorID>* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS];
      33             : 
      34             : static void handle_arm_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer &transfer, const dronecan_remoteid_ArmStatus &msg);
      35             : 
      36           0 : void AP_OpenDroneID::dronecan_init(AP_DroneCAN *uavcan)
      37             : {
      38           0 :     const uint8_t driver_index = uavcan->get_driver_index();
      39           0 :     driver_mask = 1U<<driver_index;
      40           0 :     if (dronecan_done_init & driver_mask) {
      41             :         // already initialised
      42           0 :         return;
      43             :     }
      44             : 
      45           0 :     dc_location[driver_index] = NEW_NOTHROW Canard::Publisher<dronecan_remoteid_Location>(uavcan->get_canard_iface());
      46           0 :     if (dc_location[driver_index] == nullptr) {
      47           0 :         goto alloc_failed;
      48             :     }
      49           0 :     dc_location[driver_index]->set_timeout_ms(20);
      50           0 :     dc_location[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
      51             : 
      52           0 :     dc_basic_id[driver_index] = NEW_NOTHROW Canard::Publisher<dronecan_remoteid_BasicID>(uavcan->get_canard_iface());
      53           0 :     if (dc_basic_id[driver_index] == nullptr) {
      54           0 :         goto alloc_failed;
      55             :     }
      56           0 :     dc_basic_id[driver_index]->set_timeout_ms(20);
      57           0 :     dc_basic_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
      58             : 
      59           0 :     dc_self_id[driver_index] = NEW_NOTHROW Canard::Publisher<dronecan_remoteid_SelfID>(uavcan->get_canard_iface());
      60           0 :     if (dc_self_id[driver_index] == nullptr) {
      61           0 :         goto alloc_failed;
      62             :     }
      63           0 :     dc_self_id[driver_index]->set_timeout_ms(20);
      64           0 :     dc_self_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
      65             : 
      66           0 :     dc_system[driver_index] = NEW_NOTHROW Canard::Publisher<dronecan_remoteid_System>(uavcan->get_canard_iface());
      67           0 :     if (dc_system[driver_index] == nullptr) {
      68           0 :         goto alloc_failed;
      69             :     }
      70           0 :     dc_system[driver_index]->set_timeout_ms(20);
      71           0 :     dc_system[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
      72             : 
      73           0 :     dc_operator_id[driver_index] = NEW_NOTHROW Canard::Publisher<dronecan_remoteid_OperatorID>(uavcan->get_canard_iface());
      74           0 :     if (dc_operator_id[driver_index] == nullptr) {
      75           0 :         goto alloc_failed;
      76             :     }
      77           0 :     dc_operator_id[driver_index]->set_timeout_ms(20);
      78           0 :     dc_operator_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW);
      79             : 
      80           0 :     if (Canard::allocate_sub_arg_callback(uavcan, &handle_arm_status, driver_index) == nullptr)
      81             :     {
      82           0 :         goto alloc_failed;
      83             :     }
      84             : 
      85           0 :     dronecan_done_init |= driver_mask;
      86           0 :     return;
      87             : 
      88           0 : alloc_failed:
      89           0 :     dronecan_init_failed |= driver_mask;
      90           0 :     GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "OpenDroneID DroneCAN alloc failed");
      91             : }
      92             : 
      93             : /*
      94             :   send pending DroneCAN OpenDroneID packets
      95             :  */
      96           0 : void AP_OpenDroneID::dronecan_send(AP_DroneCAN *uavcan)
      97             : {
      98           0 :     dronecan_init(uavcan);
      99             : 
     100           0 :     if (dronecan_init_failed & driver_mask) {
     101           0 :         return;
     102             :     }
     103             : 
     104           0 :     if (need_send_basic_id & driver_mask) {
     105           0 :         WITH_SEMAPHORE(_sem);
     106           0 :         dronecan_send_basic_id(uavcan);
     107           0 :         need_send_basic_id &= ~driver_mask;
     108           0 :     }
     109           0 :     if (need_send_system & driver_mask) {
     110           0 :         WITH_SEMAPHORE(_sem);
     111           0 :         dronecan_send_system(uavcan);
     112           0 :         need_send_system &= ~driver_mask;
     113           0 :     }
     114           0 :     if (need_send_self_id & driver_mask) {
     115           0 :         WITH_SEMAPHORE(_sem);
     116           0 :         dronecan_send_self_id(uavcan);
     117           0 :         need_send_self_id &= ~driver_mask;
     118           0 :     }
     119           0 :     if (need_send_operator_id & driver_mask) {
     120           0 :         WITH_SEMAPHORE(_sem);
     121           0 :         dronecan_send_operator_id(uavcan);
     122           0 :         need_send_operator_id &= ~driver_mask;
     123           0 :     }
     124           0 :     if (need_send_location & driver_mask) {
     125           0 :         WITH_SEMAPHORE(_sem);
     126           0 :         dronecan_send_location(uavcan);
     127           0 :         need_send_location &= ~driver_mask;
     128           0 :     }
     129             : }
     130             : 
     131             : #define ODID_COPY(name) msg.name = pkt.name
     132             : #define ODID_COPY_STR(name) do { msg.name.len = strncpy_noterm((char*)msg.name.data, (const char*)pkt.name, sizeof(msg.name.data)); } while(0)
     133             : 
     134             : 
     135           0 : void AP_OpenDroneID::dronecan_send_location(AP_DroneCAN *uavcan)
     136             : {
     137           0 :     dronecan_remoteid_Location msg {};
     138           0 :     const auto &pkt = pkt_location;
     139           0 :     ODID_COPY_STR(id_or_mac);
     140           0 :     ODID_COPY(status);
     141           0 :     ODID_COPY(direction);
     142           0 :     ODID_COPY(speed_horizontal);
     143           0 :     ODID_COPY(speed_vertical);
     144           0 :     ODID_COPY(latitude);
     145           0 :     ODID_COPY(longitude);
     146           0 :     ODID_COPY(altitude_barometric);
     147           0 :     ODID_COPY(altitude_geodetic);
     148           0 :     ODID_COPY(height_reference);
     149           0 :     ODID_COPY(height);
     150           0 :     ODID_COPY(horizontal_accuracy);
     151           0 :     ODID_COPY(vertical_accuracy);
     152           0 :     ODID_COPY(barometer_accuracy);
     153           0 :     ODID_COPY(speed_accuracy);
     154           0 :     ODID_COPY(timestamp);
     155           0 :     ODID_COPY(timestamp_accuracy);
     156           0 :     dc_location[uavcan->get_driver_index()]->broadcast(msg);
     157           0 : }
     158             : 
     159           0 : void AP_OpenDroneID::dronecan_send_basic_id(AP_DroneCAN *uavcan)
     160             : {
     161           0 :     dronecan_remoteid_BasicID msg {};
     162           0 :     const auto &pkt = pkt_basic_id;
     163           0 :     ODID_COPY_STR(id_or_mac);
     164           0 :     ODID_COPY(id_type);
     165           0 :     ODID_COPY(ua_type);
     166           0 :     ODID_COPY_STR(uas_id);
     167           0 :     dc_basic_id[uavcan->get_driver_index()]->broadcast(msg);
     168           0 : }
     169             : 
     170           0 : void AP_OpenDroneID::dronecan_send_system(AP_DroneCAN *uavcan)
     171             : {
     172           0 :     dronecan_remoteid_System msg {};
     173           0 :     const auto &pkt = pkt_system;
     174           0 :     ODID_COPY_STR(id_or_mac);
     175           0 :     ODID_COPY(operator_location_type);
     176           0 :     ODID_COPY(classification_type);
     177           0 :     ODID_COPY(operator_latitude);
     178           0 :     ODID_COPY(operator_longitude);
     179           0 :     ODID_COPY(area_count);
     180           0 :     ODID_COPY(area_radius);
     181           0 :     ODID_COPY(area_ceiling);
     182           0 :     ODID_COPY(area_floor);
     183           0 :     ODID_COPY(category_eu);
     184           0 :     ODID_COPY(class_eu);
     185           0 :     ODID_COPY(operator_altitude_geo);
     186           0 :     ODID_COPY(timestamp);
     187           0 :     dc_system[uavcan->get_driver_index()]->broadcast(msg);
     188           0 : }
     189             : 
     190           0 : void AP_OpenDroneID::dronecan_send_self_id(AP_DroneCAN *uavcan)
     191             : {
     192           0 :     dronecan_remoteid_SelfID msg {};
     193           0 :     const auto &pkt = pkt_self_id;
     194           0 :     ODID_COPY_STR(id_or_mac);
     195           0 :     ODID_COPY(description_type);
     196           0 :     ODID_COPY_STR(description);
     197           0 :     dc_self_id[uavcan->get_driver_index()]->broadcast(msg);
     198           0 : }
     199             : 
     200           0 : void AP_OpenDroneID::dronecan_send_operator_id(AP_DroneCAN *uavcan)
     201             : {
     202           0 :     dronecan_remoteid_OperatorID msg {};
     203           0 :     const auto &pkt = pkt_operator_id;
     204           0 :     ODID_COPY_STR(id_or_mac);
     205           0 :     ODID_COPY(operator_id_type);
     206           0 :     ODID_COPY_STR(operator_id);
     207           0 :     dc_operator_id[uavcan->get_driver_index()]->broadcast(msg);
     208           0 : }
     209             : 
     210             : /*
     211             :   handle ArmStatus message from DroneCAN
     212             :  */
     213           0 : static void handle_arm_status(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_remoteid_ArmStatus &msg)
     214             : {
     215           0 :     mavlink_open_drone_id_arm_status_t status {};
     216             : 
     217           0 :     status.status = msg.status;
     218           0 :     strncpy_noterm((char*)status.error, (const char*)msg.error.data, sizeof(status.error));
     219             : 
     220           0 :     AP::opendroneid().set_arm_status(status);
     221             : 
     222             :     // Push DroneCAN Arm Message to GCS
     223           0 :     gcs().send_to_active_channels(MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS,(const char*)&status);
     224           0 : }
     225             : 
     226             : // copy arm status for DroneCAN
     227           0 : void AP_OpenDroneID::set_arm_status(mavlink_open_drone_id_arm_status_t &status)
     228             : {
     229           0 :     WITH_SEMAPHORE(_sem);
     230           0 :     arm_status = status;
     231           0 :     last_arm_status_ms = AP_HAL::millis();
     232           0 : }
     233             : 
     234             : #endif // HAL_ENABLE_DRONECAN_DRIVERS
     235             : #endif // AP_OPENDRONEID_ENABLED