mavros_uas.h
Go to the documentation of this file.
1 
10 /*
11  * Copyright 2014,2015,2016,2017 Vladimir Ermakov.
12  *
13  * This file is part of the mavros package and subject to the license terms
14  * in the top-level LICENSE file of the mavros repository.
15  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
16  */
17 
18 #pragma once
19 
20 #include <array>
21 #include <mutex>
22 #include <atomic>
23 #include <type_traits>
29 #include <mavconn/interface.h>
30 #include <mavros/utils.h>
31 #include <mavros/frame_tf.h>
32 
33 #include <GeographicLib/Geoid.hpp>
34 
35 #include <sensor_msgs/Imu.h>
36 #include <sensor_msgs/NavSatFix.h>
37 
38 namespace mavros {
42 #define UAS_FCU(uasobjptr) \
43  ((uasobjptr)->fcu_link)
44 
48 #define UAS_DIAG(uasobjptr) \
49  ((uasobjptr)->diag_updater)
50 
51 
67 class UAS {
68 public:
69  // common enums used by UAS
70  using MAV_TYPE = mavlink::minimal::MAV_TYPE;
71  using MAV_AUTOPILOT = mavlink::minimal::MAV_AUTOPILOT;
72  using MAV_MODE_FLAG = mavlink::minimal::MAV_MODE_FLAG;
73  using MAV_STATE = mavlink::minimal::MAV_STATE;
74  using MAV_CAP = mavlink::common::MAV_PROTOCOL_CAPABILITY;
76 
77  // other UAS aliases
78  using ConnectionCb = std::function<void(bool)>;
79  using CapabilitiesCb = std::function<void(MAV_CAP)>;
80  using lock_guard = std::lock_guard<std::recursive_mutex>;
81  using unique_lock = std::unique_lock<std::recursive_mutex>;
82 
83  UAS();
84  ~UAS() {};
85 
90 
95 
99  inline bool is_connected() {
100  return connected;
101  }
102 
103  /* -*- HEARTBEAT data -*- */
104 
108  void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_);
109 
113  void update_connection_status(bool conn_);
114 
119 
123  inline MAV_TYPE get_type() {
124  std::underlying_type<MAV_TYPE>::type type_ = type;
125  return static_cast<MAV_TYPE>(type_);
126  }
127 
132  std::underlying_type<MAV_AUTOPILOT>::type autopilot_ = autopilot;
133  return static_cast<MAV_AUTOPILOT>(autopilot_);
134  }
135 
141  inline bool get_armed() {
142  uint8_t base_mode_ = base_mode;
143  return base_mode_ & utils::enum_value(MAV_MODE_FLAG::SAFETY_ARMED);
144  }
145 
149  inline bool get_hil_state() {
150  uint8_t base_mode_ = base_mode;
151  return base_mode_ & utils::enum_value(MAV_MODE_FLAG::HIL_ENABLED);
152  }
153 
154  /* -*- FCU target id pair -*- */
155 
159  inline uint8_t get_tgt_system() {
160  return target_system; // not changed after configuration
161  }
162 
166  inline uint8_t get_tgt_component() {
167  return target_component;// not changed after configuration
168  }
169 
170  inline void set_tgt(uint8_t sys, uint8_t comp) {
171  target_system = sys;
172  target_component = comp;
173  }
174 
175 
176  /* -*- IMU data -*- */
177 
182 
187 
192 
197 
202  geometry_msgs::Quaternion get_attitude_orientation_enu();
203 
208  geometry_msgs::Quaternion get_attitude_orientation_ned();
209 
214  geometry_msgs::Vector3 get_attitude_angular_velocity_enu();
215 
220  geometry_msgs::Vector3 get_attitude_angular_velocity_ned();
221 
222 
223  /* -*- GPS data -*- */
224 
227  float eph, float epv,
228  int fix_type, int satellites_visible);
229 
231  void get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible);
232 
235 
236  /* -*- GograpticLib utils -*- */
237 
243  std::shared_ptr<GeographicLib::Geoid> egm96_5;
244 
249  template <class T>
250  inline double geoid_to_ellipsoid_height(T lla)
251  {
252  if (egm96_5)
253  return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude);
254  else
255  return 0.0;
256  }
257 
262  template <class T>
263  inline double ellipsoid_to_geoid_height(T lla)
264  {
265  if (egm96_5)
266  return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude);
267  else
268  return 0.0;
269  }
270 
271  /* -*- transform -*- */
272 
277 
286  void add_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr, std::vector<geometry_msgs::TransformStamped>& vector);
287 
295  void publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr);
296 
297  /* -*- time sync -*- */
298 
299  inline void set_time_offset(uint64_t offset_ns) {
300  time_offset = offset_ns;
301  }
302 
303  inline uint64_t get_time_offset(void) {
304  return time_offset;
305  }
306 
307  inline void set_timesync_mode(timesync_mode mode) {
308  tsync_mode = mode;
309  }
310 
312  return tsync_mode;
313  }
314 
315  /* -*- autopilot version -*- */
316  uint64_t get_capabilities();
317 
323  template<typename T>
324  bool has_capability(T capability){
325  static_assert(std::is_enum<T>::value, "Only query capabilities using the UAS::MAV_CAP enum.");
326  return get_capabilities() & utils::enum_value(capability);
327  }
328 
335  template<typename ... Ts>
336  bool has_capabilities(Ts ... capabilities){
337  bool ret = true;
338  std::initializer_list<bool> capabilities_list{has_capability<Ts>(capabilities) ...};
339  for (auto has_cap : capabilities_list) ret &= has_cap;
340  return ret;
341  }
342 
346  void update_capabilities(bool known, uint64_t caps = 0);
347 
354 
362  ros::Time synchronise_stamp(uint32_t time_boot_ms);
363  ros::Time synchronise_stamp(uint64_t time_usec);
364 
374  template<typename T>
375  inline std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp) {
376  std_msgs::Header out;
377  out.frame_id = frame_id;
378  out.stamp = synchronise_stamp(time_stamp);
379  return out;
380  }
381 
382  /* -*- utils -*- */
383 
387  template<typename _T>
388  inline void msg_set_target(_T &msg) {
389  msg.target_system = get_tgt_system();
390  msg.target_component = get_tgt_component();
391  }
392 
396  inline bool is_my_target(uint8_t sysid, uint8_t compid) {
397  return sysid == get_tgt_system() && compid == get_tgt_component();
398  }
399 
403  inline bool is_my_target(uint8_t sysid) {
404  return sysid == get_tgt_system();
405  }
406 
410  inline bool is_ardupilotmega() {
411  return MAV_AUTOPILOT::ARDUPILOTMEGA == get_autopilot();
412  }
413 
417  inline bool is_px4() {
418  return MAV_AUTOPILOT::PX4 == get_autopilot();
419  }
420 
434  std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode);
435 
445  bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode);
446 
447 private:
448  std::recursive_mutex mutex;
449 
450  std::atomic<uint8_t> type;
451  std::atomic<uint8_t> autopilot;
452  std::atomic<uint8_t> base_mode;
453 
454  uint8_t target_system;
456 
457  std::atomic<bool> connected;
458  std::vector<ConnectionCb> connection_cb_vec;
459  std::vector<CapabilitiesCb> capabilities_cb_vec;
460 
463 
465  float gps_eph;
466  float gps_epv;
469 
470  std::atomic<uint64_t> time_offset;
472 
473  std::atomic<bool> fcu_caps_known;
474  std::atomic<uint64_t> fcu_capabilities;
475 };
476 } // namespace mavros
bool is_connected()
Return connection status.
Definition: mavros_uas.h:99
std::atomic< uint8_t > autopilot
Definition: mavros_uas.h:451
uint8_t target_component
Definition: mavros_uas.h:455
bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode)
Lookup custom mode for given string.
mavlink::minimal::MAV_TYPE MAV_TYPE
Definition: mavros_uas.h:70
mavconn::MAVConnInterface::Ptr fcu_link
MAVLink FCU device conection.
Definition: mavros_uas.h:84
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
Create message header from time_boot_ms or time_usec stamps and frame_id.
Definition: mavros_uas.h:375
sensor_msgs::NavSatFix::Ptr gps_fix
Definition: mavros_uas.h:464
double ellipsoid_to_geoid_height(T lla)
Conversion from height above ellipsoid (WGS-84) to height above geoid (AMSL)
Definition: mavros_uas.h:263
void add_capabilities_change_handler(CapabilitiesCb cb)
Adds a function to the capabilities callback queue.
Definition: uas_data.cpp:133
bool has_capabilities(Ts...capabilities)
Function to check if the flight controller has a set of capabilities.
Definition: mavros_uas.h:336
void publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr)
Publishes static transform.
Definition: uas_data.cpp:267
tf2_ros::TransformListener tf2_listener
Definition: mavros_uas.h:274
geometry_msgs::Quaternion get_attitude_orientation_enu()
Get Attitude orientation quaternion.
Definition: uas_data.cpp:165
uint64_t get_time_offset(void)
Definition: mavros_uas.h:303
MAV_AUTOPILOT get_autopilot()
Returns autopilot type.
Definition: mavros_uas.h:131
mavlink::minimal::MAV_STATE MAV_STATE
Definition: mavros_uas.h:73
void update_connection_status(bool conn_)
Definition: uas_data.cpp:72
float gps_epv
Definition: mavros_uas.h:466
void set_time_offset(uint64_t offset_ns)
Definition: mavros_uas.h:299
diagnostic_updater::Updater diag_updater
Mavros diagnostic updater.
Definition: mavros_uas.h:94
sensor_msgs::Imu::Ptr imu_enu_data
Definition: mavros_uas.h:461
void msg_set_target(_T &msg)
Definition: mavros_uas.h:388
bool has_capability(T capability)
Function to check if the flight controller has a capability.
Definition: mavros_uas.h:324
tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster
Definition: mavros_uas.h:276
bool get_hil_state()
Returns HIL status.
Definition: mavros_uas.h:149
std::function< void(bool)> ConnectionCb
Definition: mavros_uas.h:78
tf2_ros::TransformBroadcaster tf2_broadcaster
Definition: mavros_uas.h:275
geometry_msgs::Quaternion get_attitude_orientation_ned()
Get Attitude orientation quaternion.
Definition: uas_data.cpp:178
void update_capabilities(bool known, uint64_t caps=0)
Update the capabilities if they&#39;ve changed every VERSION/timeout.
Definition: uas_data.cpp:109
void update_attitude_imu_enu(sensor_msgs::Imu::Ptr &imu)
Store IMU data [ENU].
Definition: uas_data.cpp:141
uint8_t get_tgt_system()
Return communication target system.
Definition: mavros_uas.h:159
UAS for plugins.
Definition: mavros_uas.h:67
std::atomic< uint8_t > type
Definition: mavros_uas.h:450
std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode)
Represent FCU mode as string.
tf2_ros::Buffer tf2_buffer
Definition: mavros_uas.h:273
std::shared_ptr< GeographicLib::Geoid > egm96_5
Geoid dataset used to convert between AMSL and WGS-84.
Definition: mavros_uas.h:243
void add_connection_change_handler(ConnectionCb cb)
Add connection change handler callback.
Definition: uas_data.cpp:83
bool is_ardupilotmega()
Check that FCU is APM.
Definition: mavros_uas.h:410
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Compute FCU message time from time_boot_ms or time_usec field.
void set_timesync_mode(timesync_mode mode)
Definition: mavros_uas.h:307
mavlink::common::MAV_PROTOCOL_CAPABILITY MAV_CAP
Definition: mavros_uas.h:74
mavlink::minimal::MAV_MODE_FLAG MAV_MODE_FLAG
Definition: mavros_uas.h:72
void set_tgt(uint8_t sys, uint8_t comp)
Definition: mavros_uas.h:170
geometry_msgs::Vector3 get_attitude_angular_velocity_ned()
Get angular velocity from IMU data.
Definition: uas_data.cpp:204
std::lock_guard< std::recursive_mutex > lock_guard
Definition: mavros_uas.h:80
std::atomic< bool > connected
Definition: mavros_uas.h:457
std::atomic< bool > fcu_caps_known
Definition: mavros_uas.h:473
std::vector< CapabilitiesCb > capabilities_cb_vec
Definition: mavros_uas.h:459
bool get_armed()
Returns arming status.
Definition: mavros_uas.h:141
MAV_TYPE get_type()
Returns vehicle type.
Definition: mavros_uas.h:123
std::function< void(MAV_CAP)> CapabilitiesCb
Definition: mavros_uas.h:79
timesync_mode tsync_mode
Definition: mavros_uas.h:471
sensor_msgs::Imu::Ptr get_attitude_imu_ned()
Get IMU data [NED].
Definition: uas_data.cpp:159
bool is_my_target(uint8_t sysid, uint8_t compid)
Check that sys/comp id&#39;s is my target.
Definition: mavros_uas.h:396
std::recursive_mutex mutex
Definition: mavros_uas.h:448
void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_)
Definition: uas_data.cpp:65
std::unique_lock< std::recursive_mutex > unique_lock
Definition: mavros_uas.h:81
int gps_fix_type
Definition: mavros_uas.h:467
sensor_msgs::NavSatFix::Ptr get_gps_fix()
Retunrs last GPS RAW message.
Definition: uas_data.cpp:245
mavlink::minimal::MAV_AUTOPILOT MAV_AUTOPILOT
Definition: mavros_uas.h:71
float gps_eph
Definition: mavros_uas.h:465
timesync_mode get_timesync_mode(void)
Definition: mavros_uas.h:311
uint64_t get_capabilities()
Definition: uas_data.cpp:97
std::atomic< uint8_t > base_mode
Definition: mavros_uas.h:452
bool is_my_target(uint8_t sysid)
Check that system id is my target.
Definition: mavros_uas.h:403
uint8_t target_system
Definition: mavros_uas.h:454
sensor_msgs::Imu::Ptr imu_ned_data
Definition: mavros_uas.h:462
void update_attitude_imu_ned(sensor_msgs::Imu::Ptr &imu)
Store IMU data [NED].
Definition: uas_data.cpp:147
std::atomic< uint64_t > time_offset
Definition: mavros_uas.h:470
sensor_msgs::Imu::Ptr get_attitude_imu_enu()
Get IMU data [ENU].
Definition: uas_data.cpp:153
Frame transformation utilities.
bool is_px4()
Check that FCU is PX4.
Definition: mavros_uas.h:417
void update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix, float eph, float epv, int fix_type, int satellites_visible)
Store GPS RAW data.
Definition: uas_data.cpp:220
std::atomic< uint64_t > fcu_capabilities
Definition: mavros_uas.h:474
uint8_t get_tgt_component()
Return communication target component.
Definition: mavros_uas.h:166
geometry_msgs::Vector3 get_attitude_angular_velocity_enu()
Get angular velocity from IMU data.
Definition: uas_data.cpp:191
double geoid_to_ellipsoid_height(T lla)
Conversion from height above geoid (AMSL) to height above ellipsoid (WGS-84)
Definition: mavros_uas.h:250
void add_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr, std::vector< geometry_msgs::TransformStamped > &vector)
Add static transform. To publish all static transforms at once, we stack them in a std::vector...
Definition: uas_data.cpp:254
int gps_satellites_visible
Definition: mavros_uas.h:468
std::vector< ConnectionCb > connection_cb_vec
Definition: mavros_uas.h:458
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
void get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible)
Returns EPH, EPV, Fix type and satellites visible.
Definition: uas_data.cpp:234
std::shared_ptr< MAVConnInterface > Ptr


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26