GCS_MAVLink.h 2.2 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576
  1. /// @file GCS_MAVLink.h
  2. /// @brief One size fits all header for MAVLink integration.
  3. #pragma once
  4. #include <AP_HAL/AP_HAL_Boards.h>
  5. // we have separate helpers disabled to make it possible
  6. // to select MAVLink 1.0 in the arduino GUI build
  7. #define MAVLINK_SEPARATE_HELPERS
  8. #define MAVLINK_NO_CONVERSION_HELPERS
  9. #define MAVLINK_SEND_UART_BYTES(chan, buf, len) comm_send_buffer(chan, buf, len)
  10. #define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan)
  11. #define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan)
  12. #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
  13. // allow extra mavlink channels in SITL for:
  14. // Vicon
  15. #define MAVLINK_COMM_NUM_BUFFERS 6
  16. #else
  17. // allow five telemetry ports
  18. #define MAVLINK_COMM_NUM_BUFFERS 5
  19. #endif
  20. /*
  21. The MAVLink protocol code generator does its own alignment, so
  22. alignment cast warnings can be ignored
  23. */
  24. #pragma GCC diagnostic push
  25. #pragma GCC diagnostic ignored "-Wcast-align"
  26. #if defined(__GNUC__) && __GNUC__ >= 9
  27. #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
  28. #endif
  29. #include "include/mavlink/v2.0/ardupilotmega/version.h"
  30. #define MAVLINK_MAX_PAYLOAD_LEN 255
  31. #include "include/mavlink/v2.0/mavlink_types.h"
  32. /// MAVLink stream used for uartA
  33. extern AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
  34. extern bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
  35. /// MAVLink system definition
  36. extern mavlink_system_t mavlink_system;
  37. /// Sanity check MAVLink channel
  38. ///
  39. /// @param chan Channel to send to
  40. static inline bool valid_channel(mavlink_channel_t chan)
  41. {
  42. #pragma clang diagnostic push
  43. #pragma clang diagnostic ignored "-Wtautological-constant-out-of-range-compare"
  44. return chan < MAVLINK_COMM_NUM_BUFFERS;
  45. #pragma clang diagnostic pop
  46. }
  47. void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);
  48. /// Check for available transmit space on the nominated MAVLink channel
  49. ///
  50. /// @param chan Channel to check
  51. /// @returns Number of bytes available
  52. uint16_t comm_get_txspace(mavlink_channel_t chan);
  53. #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
  54. #include "include/mavlink/v2.0/ardupilotmega/mavlink.h"
  55. // lock and unlock a channel, for multi-threaded mavlink send
  56. void comm_send_lock(mavlink_channel_t chan);
  57. void comm_send_unlock(mavlink_channel_t chan);
  58. #pragma GCC diagnostic pop