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             :  * Code by:
      16             :  *  BlueMark Innovations BV, Roel Schiphorst
      17             :  *  Contributors: Tom Pittenger, Josh Henderson
      18             :  *  Parts of this code are based on/copied from the Open Drone ID project https://github.com/opendroneid/opendroneid-core-c
      19             :  *
      20             :  * The code has been tested with the BlueMark DroneBeacon MAVLink transponder running this command in the ArduPlane folder:
      21             :  * sim_vehicle.py --wipe-eeprom --console --map -A --serial1=uart:/dev/ttyUSB1:9600
      22             :  * (and a DroneBeacon MAVLink transponder connected to ttyUSB1)
      23             :  *
      24             :  * The Remote ID implementation expects a transponder that caches the received MAVLink messages from ArduPilot
      25             :  * and transmits them at the required intervals. So static messages are only sent once to the transponder.
      26             :  */
      27             : 
      28             : #pragma once
      29             : 
      30             : #include "AP_OpenDroneID_config.h"
      31             : 
      32             : #if AP_OPENDRONEID_ENABLED
      33             : 
      34             : #include <AP_Math/AP_Math.h>
      35             : #include <AP_Param/AP_Param.h>
      36             : #include <GCS_MAVLink/GCS_MAVLink.h>
      37             : #include <AP_Common/Location.h>
      38             : 
      39             : #define ODID_ID_SIZE 20
      40             : #define ODID_STR_SIZE 23
      41             : 
      42             : #define ODID_MIN_DIR         0       // Minimum direction
      43             : #define ODID_MAX_DIR         360     // Maximum direction
      44             : #define ODID_INV_DIR         361     // Invalid direction
      45             : #define ODID_MIN_SPEED_H     0       // Minimum speed horizontal
      46             : #define ODID_MAX_SPEED_H     254.25f // Maximum speed horizontal
      47             : #define ODID_INV_SPEED_H     255     // Invalid speed horizontal
      48             : #define ODID_MIN_SPEED_V     (-62)   // Minimum speed vertical
      49             : #define ODID_MAX_SPEED_V     62      // Maximum speed vertical
      50             : #define ODID_INV_SPEED_V     63      // Invalid speed vertical
      51             : #define ODID_MIN_ALT         (-1000) // Minimum altitude
      52             : #define ODID_MAX_ALT         31767.5f// Maximum altitude
      53             : #define ODID_INV_ALT         ODID_MIN_ALT // Invalid altitude
      54             : #define ODID_MAX_TIMESTAMP   (60 * 60)
      55             : #define ODID_INV_TIMESTAMP   0xFFFF  // Invalid, No Value or Unknown Timestamp
      56             : #define ODID_MAX_AREA_RADIUS 2550
      57             : #define ODID_AREA_COUNT_MIN  1
      58             : #define ODID_AREA_COUNT_MAX  65000
      59             : 
      60             : class AP_DroneCAN;
      61             : 
      62             : class AP_OpenDroneID
      63             : {
      64             : public:
      65             :     AP_OpenDroneID();
      66             : 
      67             :     /* Do not allow copies */
      68             :     CLASS_NO_COPY(AP_OpenDroneID);
      69             : 
      70             :     // parameter block
      71             :     static const struct AP_Param::GroupInfo var_info[];
      72             : 
      73             :     void init();
      74             :     bool pre_arm_check(char* failmsg, uint8_t failmsg_len);
      75             :     void update();
      76             : 
      77             :     // send pending dronecan messages
      78             :     void dronecan_send(AP_DroneCAN *);
      79             : 
      80             :     // handle a message from the GCS
      81             :     void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg);
      82             : 
      83    24742308 :     bool enabled(void) const {
      84    24742308 :         return _enable != 0;
      85             :     }
      86             : 
      87             :     void set_arm_status(mavlink_open_drone_id_arm_status_t &status);
      88             : 
      89             :     void set_basic_id();
      90             : 
      91             :     void get_persistent_params(ExpandingString &str) const;
      92             : 
      93             :     void load_UAS_ID_from_persistent_memory();
      94             : 
      95             :     // get singleton instance
      96    49625317 :     static AP_OpenDroneID *get_singleton()
      97             :     {
      98    49625317 :         return _singleton;
      99             :     }
     100             : private:
     101             :     static AP_OpenDroneID *_singleton;
     102             :     bool _initialised;
     103             :     // parameters
     104             :     AP_Int8  _enable;
     105             :     AP_Float _baro_accuracy;    // Vertical accuracy of the barometer when installed
     106             :     AP_Int16 _options;
     107             :     AP_Int8  _mav_port;
     108             :     AP_Int8  _can_driver;
     109             : 
     110             :     char ua_type[3];
     111             :     char id_type[3];
     112             :     size_t id_len;
     113             :     char id_str[21];
     114             :     bool bootloader_flashed;
     115             :     enum Options : int16_t {
     116             :         EnforcePreArmChecks = (1U << 0U),
     117             :         AllowNonGPSPosition = (1U << 1U),
     118             :         LockUASIDOnFirstBasicIDRx = (1U << 2U),
     119             :     };
     120             : 
     121             :     // check if an option is set
     122      140684 :     bool option_enabled(const Options option) const
     123             :     {
     124      140684 :         return (uint8_t(_options.get()) & uint8_t(option)) != 0;
     125             :     }
     126             : 
     127             :     mavlink_channel_t _chan; // MAVLink channel that communicates with the Remote ID Transceiver
     128             :     const mavlink_channel_t MAV_CHAN_INVALID = mavlink_channel_t(255U);
     129             :     uint32_t _last_send_location_ms;
     130             :     uint32_t _last_send_system_update_ms;
     131             :     uint32_t _last_send_static_messages_ms;
     132             :     static constexpr uint32_t _mavlink_dynamic_period_ms = 1000; //how often are mavlink dynamic messages sent in ms. E.g. 1000 = 1 Hz
     133             :     static constexpr uint32_t _mavlink_static_period_ms = 3000; //how often are mavlink static messages sent in ms
     134             : 
     135             :     bool     _have_height_above_takeoff;
     136             :     Location _takeoff_location;
     137             :     bool _was_armed;
     138             : 
     139             :     // packets ready to be sent, updated with semaphore held
     140             :     HAL_Semaphore _sem;
     141             :     mavlink_open_drone_id_location_t pkt_location;
     142             :     mavlink_open_drone_id_basic_id_t pkt_basic_id;
     143             :     mavlink_open_drone_id_system_t pkt_system;
     144             :     mavlink_open_drone_id_self_id_t pkt_self_id;
     145             :     mavlink_open_drone_id_operator_id_t pkt_operator_id;
     146             : 
     147             :     // last time we got a SYSTEM message
     148             :     uint32_t last_system_ms;
     149             : 
     150             :     // last time we got a SYSTEM_UPDATE message
     151             :     uint32_t last_system_update_ms;
     152             : 
     153             :     // arm status from the transmitter
     154             :     mavlink_open_drone_id_arm_status_t arm_status;
     155             :     uint32_t last_arm_status_ms;
     156             : 
     157             :     // last time we sent a lost transmitter message
     158             :     uint32_t last_lost_tx_ms;
     159             : 
     160             :     // last time we sent a lost operator location notice
     161             :     uint32_t last_lost_operator_msg_ms;
     162             :     
     163             :     // transmit functions to manually send a static MAVLink message
     164             :     void send_dynamic_out();
     165             :     void send_static_out();
     166             :     void send_basic_id_message();
     167             :     void send_system_message();
     168             :     void send_system_update_message();
     169             :     void send_self_id_message();
     170             :     void send_operator_id_message();
     171             :     void send_location_message();
     172             :     enum next_msg : uint8_t {
     173             :         NEXT_MSG_BASIC_ID = 0,
     174             :         NEXT_MSG_SYSTEM,
     175             :         NEXT_MSG_SELF_ID,
     176             :         NEXT_MSG_OPERATOR_ID,
     177             :         NEXT_MSG_ENUM_END
     178             :     } next_msg_to_send;
     179             :     uint32_t last_msg_send_ms;
     180             : 
     181             :     // helper functions
     182             :     MAV_ODID_HOR_ACC create_enum_horizontal_accuracy(float Accuracy) const;
     183             :     MAV_ODID_VER_ACC create_enum_vertical_accuracy(float Accuracy) const;
     184             :     MAV_ODID_SPEED_ACC create_enum_speed_accuracy(float Accuracy) const;
     185             :     MAV_ODID_TIME_ACC create_enum_timestamp_accuracy(float Accuracy) const;
     186             :     uint16_t create_direction(uint16_t direction) const;
     187             :     uint16_t create_speed_horizontal(uint16_t speed) const;
     188             :     int16_t create_speed_vertical(int16_t speed) const;
     189             :     float create_altitude(float altitude) const;
     190             :     float create_location_timestamp(float timestamp) const;
     191             : 
     192             :     // mask of what UAVCAN drivers need to send each packet
     193             :     const uint8_t dronecan_send_all = (1U<<HAL_MAX_CAN_PROTOCOL_DRIVERS)-1;
     194             :     uint8_t driver_mask;
     195             :     uint8_t need_send_location;
     196             :     uint8_t need_send_basic_id;
     197             :     uint8_t need_send_system;
     198             :     uint8_t need_send_self_id;
     199             :     uint8_t need_send_operator_id;
     200             : 
     201             :     uint8_t dronecan_done_init;
     202             :     uint8_t dronecan_init_failed;
     203             :     void dronecan_init(AP_DroneCAN *uavcan);
     204             :     void dronecan_send_location(AP_DroneCAN *uavcan);
     205             :     void dronecan_send_basic_id(AP_DroneCAN *uavcan);
     206             :     void dronecan_send_system(AP_DroneCAN *uavcan);
     207             :     void dronecan_send_self_id(AP_DroneCAN *uavcan);
     208             :     void dronecan_send_operator_id(AP_DroneCAN *uavcan);
     209             : };
     210             : 
     211             : namespace AP
     212             : {
     213             : AP_OpenDroneID &opendroneid();
     214             : };
     215             : 
     216             : #endif // AP_OPENDRONEID_ENABLED