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             : /// @file       GCS_MAVLink.cpp
      17             : 
      18             : /*
      19             : This provides some support code and variables for MAVLink enabled sketches
      20             : 
      21             : */
      22             : 
      23             : #include "GCS_config.h"
      24             : 
      25             : #if HAL_MAVLINK_BINDINGS_ENABLED
      26             : 
      27             : #include "GCS.h"
      28             : #include "GCS_MAVLink.h"
      29             : 
      30             : #include <AP_Common/AP_Common.h>
      31             : #include <AP_HAL/AP_HAL.h>
      32             : 
      33             : extern const AP_HAL::HAL& hal;
      34             : 
      35             : #ifdef MAVLINK_SEPARATE_HELPERS
      36             : // Shut up warnings about missing declarations; TODO: should be fixed on
      37             : // mavlink/pymavlink project for when MAVLINK_SEPARATE_HELPERS is defined
      38             : #pragma GCC diagnostic push
      39             : #pragma GCC diagnostic ignored "-Wmissing-declarations"
      40             : #include "include/mavlink/v2.0/mavlink_helpers.h"
      41             : #pragma GCC diagnostic pop
      42             : #endif
      43             : 
      44           0 : mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) {
      45             : #if HAL_GCS_ENABLED
      46           0 :     GCS_MAVLINK *link = gcs().chan(chan);
      47           0 :     if (link == nullptr) {
      48           0 :         return nullptr;
      49             :     }
      50           0 :     return link->channel_buffer();
      51             : #else
      52             :     return nullptr;
      53             : #endif
      54             : }
      55             : 
      56   314543847 : mavlink_status_t* mavlink_get_channel_status(uint8_t chan) {
      57             : #if HAL_GCS_ENABLED
      58   314543847 :     GCS_MAVLINK *link = gcs().chan(chan);
      59   314543847 :     if (link == nullptr) {
      60    17544946 :         return nullptr;
      61             :     }
      62   296998901 :     return link->channel_status();
      63             : #else
      64             :     return nullptr;
      65             : #endif
      66             : }
      67             : 
      68             : #endif // HAL_MAVLINK_BINDINGS_ENABLED
      69             : 
      70             : #if HAL_GCS_ENABLED
      71             : 
      72             : AP_HAL::UARTDriver      *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
      73             : bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
      74             : 
      75             : // per-channel lock
      76             : static HAL_Semaphore chan_locks[MAVLINK_COMM_NUM_BUFFERS];
      77             : static bool chan_discard[MAVLINK_COMM_NUM_BUFFERS];
      78             : 
      79             : mavlink_system_t mavlink_system = {7,1};
      80             : 
      81             : // routing table
      82             : MAVLink_routing GCS_MAVLINK::routing;
      83             : 
      84           8 : GCS_MAVLINK *GCS_MAVLINK::find_by_mavtype_and_compid(uint8_t mav_type, uint8_t compid, uint8_t &sysid) {
      85             :     mavlink_channel_t channel;
      86           8 :     if (!routing.find_by_mavtype_and_compid(mav_type, compid, sysid, channel)) {
      87           0 :         return nullptr;
      88             :     }
      89           8 :     return gcs().chan(channel);
      90             : }
      91             : 
      92             : // set a channel as private. Private channels get sent heartbeats, but
      93             : // don't get broadcast packets or forwarded packets
      94       18206 : void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan)
      95             : {
      96       18206 :     const uint8_t mask = (1U<<(unsigned)_chan);
      97       18206 :     mavlink_private |= mask;
      98       18206 : }
      99             : 
     100             : // return a MAVLink parameter type given a AP_Param type
     101     1541876 : MAV_PARAM_TYPE GCS_MAVLINK::mav_param_type(enum ap_var_type t)
     102             : {
     103     1541876 :     if (t == AP_PARAM_INT8) {
     104      477115 :             return MAV_PARAM_TYPE_INT8;
     105             :     }
     106     1064761 :     if (t == AP_PARAM_INT16) {
     107      303053 :             return MAV_PARAM_TYPE_INT16;
     108             :     }
     109      761708 :     if (t == AP_PARAM_INT32) {
     110      133160 :             return MAV_PARAM_TYPE_INT32;
     111             :     }
     112             :     // treat any others as float
     113      628548 :     return MAV_PARAM_TYPE_REAL32;
     114             : }
     115             : 
     116             : 
     117             : /// Check for available transmit space on the nominated MAVLink channel
     118             : ///
     119             : /// @param chan         Channel to check
     120             : /// @returns            Number of bytes available
     121   105695018 : uint16_t comm_get_txspace(mavlink_channel_t chan)
     122             : {
     123   105695018 :     GCS_MAVLINK *link = gcs().chan(chan);
     124   105695018 :     if (link == nullptr) {
     125           0 :         return 0;
     126             :     }
     127   105695018 :     return link->txspace();
     128             : }
     129             : 
     130             : /*
     131             :   send a buffer out a MAVLink channel
     132             :  */
     133   106074534 : void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
     134             : {
     135   106074534 :     if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr || chan_discard[chan]) {
     136           0 :         return;
     137             :     }
     138             : #if HAL_HIGH_LATENCY2_ENABLED
     139             :     // if it's a disabled high latency channel, don't send
     140   106074534 :     GCS_MAVLINK *link = gcs().chan(chan);
     141   106074534 :     if (link->is_high_latency_link && !gcs().get_high_latency_status()) {
     142        5508 :         return;
     143             :     }
     144             : #endif
     145   106069026 :     if (gcs_alternative_active[chan]) {
     146             :         // an alternative protocol is active
     147           0 :         return;
     148             :     }
     149   106069026 :     const size_t written = mavlink_comm_port[chan]->write(buf, len);
     150             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     151   106069026 :     if (written < len && !mavlink_comm_port[chan]->is_write_locked()) {
     152           0 :         AP_HAL::panic("Short write on UART: %lu < %u", (unsigned long)written, len);
     153             :     }
     154             : #else
     155             :     (void)written;
     156             : #endif
     157             : }
     158             : 
     159             : /*
     160             :   lock a channel for send
     161             :   if there is insufficient space to send size bytes then all bytes
     162             :   written to the channel by the mavlink library will be discarded
     163             :   while the lock is held.
     164             :  */
     165    35358178 : void comm_send_lock(mavlink_channel_t chan_m, uint16_t size)
     166             : {
     167    35358178 :     const uint8_t chan = uint8_t(chan_m);
     168    35358178 :     chan_locks[chan].take_blocking();
     169    35358178 :     if (mavlink_comm_port[chan]->txspace() < size) {
     170           0 :         chan_discard[chan] = true;
     171           0 :         gcs_out_of_space_to_send(chan_m);
     172             :     }
     173    35358178 : }
     174             : 
     175             : /*
     176             :   unlock a channel
     177             :  */
     178    35358178 : void comm_send_unlock(mavlink_channel_t chan_m)
     179             : {
     180    35358178 :     const uint8_t chan = uint8_t(chan_m);
     181    35358178 :     chan_discard[chan] = false;
     182    35358178 :     chan_locks[chan].give();
     183    35358178 : }
     184             : 
     185             : /*
     186             :   return reference to GCS channel lock, allowing for
     187             :   HAVE_PAYLOAD_SPACE() to be run with a locked channel
     188             :  */
     189   173367476 : HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan)
     190             : {
     191   173367476 :     return chan_locks[uint8_t(chan)];
     192             : }
     193             : 
     194             : #endif  // HAL_GCS_ENABLED