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>
28 #include <mavconn/interface.h>
29 #include <mavros/utils.h>
30 #include <mavros/frame_tf.h>
31 
32 #include <GeographicLib/Geoid.hpp>
33 
34 #include <sensor_msgs/Imu.h>
35 #include <sensor_msgs/NavSatFix.h>
36 
37 namespace mavros {
41 #define UAS_FCU(uasobjptr) \
42  ((uasobjptr)->fcu_link)
43 
47 #define UAS_DIAG(uasobjptr) \
48  ((uasobjptr)->diag_updater)
49 
50 
66 class UAS {
67 public:
68  using ConnectionCb = std::function<void(bool)>;
69  using lock_guard = std::lock_guard<std::recursive_mutex>;
70  using unique_lock = std::unique_lock<std::recursive_mutex>;
71 
72  // common enums used by UAS
73  using MAV_TYPE = mavlink::common::MAV_TYPE;
74  using MAV_AUTOPILOT = mavlink::common::MAV_AUTOPILOT;
75  using MAV_MODE_FLAG = mavlink::common::MAV_MODE_FLAG;
76  using MAV_STATE = mavlink::common::MAV_STATE;
78 
79  UAS();
80  ~UAS() {};
81 
86 
91 
95  inline bool is_connected() {
96  return connected;
97  }
98 
99  /* -*- HEARTBEAT data -*- */
100 
104  void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_);
105 
109  void update_connection_status(bool conn_);
110 
115 
119  inline MAV_TYPE get_type() {
120  std::underlying_type<MAV_TYPE>::type type_ = type;
121  return static_cast<MAV_TYPE>(type_);
122  }
123 
128  std::underlying_type<MAV_AUTOPILOT>::type autopilot_ = autopilot;
129  return static_cast<MAV_AUTOPILOT>(autopilot_);
130  }
131 
137  inline bool get_armed() {
138  uint8_t base_mode_ = base_mode;
139  return base_mode_ & utils::enum_value(MAV_MODE_FLAG::SAFETY_ARMED);
140  }
141 
145  inline bool get_hil_state() {
146  uint8_t base_mode_ = base_mode;
147  return base_mode_ & utils::enum_value(MAV_MODE_FLAG::HIL_ENABLED);
148  }
149 
150  /* -*- FCU target id pair -*- */
151 
155  inline uint8_t get_tgt_system() {
156  return target_system; // not changed after configuration
157  }
158 
162  inline uint8_t get_tgt_component() {
163  return target_component;// not changed after configuration
164  }
165 
166  inline void set_tgt(uint8_t sys, uint8_t comp) {
167  target_system = sys;
168  target_component = comp;
169  }
170 
171 
172  /* -*- IMU data -*- */
173 
178 
183 
188 
193 
198  geometry_msgs::Quaternion get_attitude_orientation_enu();
199 
204  geometry_msgs::Quaternion get_attitude_orientation_ned();
205 
210  geometry_msgs::Vector3 get_attitude_angular_velocity_enu();
211 
216  geometry_msgs::Vector3 get_attitude_angular_velocity_ned();
217 
218 
219  /* -*- GPS data -*- */
220 
223  float eph, float epv,
224  int fix_type, int satellites_visible);
225 
227  void get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible);
228 
231 
232  /* -*- GograpticLib utils -*- */
233 
239  std::shared_ptr<GeographicLib::Geoid> egm96_5;
240 
245  template <class T>
246  inline double geoid_to_ellipsoid_height(T lla)
247  {
248  if (egm96_5)
249  return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude);
250  else
251  return 0.0;
252  }
253 
258  template <class T>
259  inline double ellipsoid_to_geoid_height(T lla)
260  {
261  if (egm96_5)
262  return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude);
263  else
264  return 0.0;
265  }
266 
267  /* -*- transform -*- */
268 
273 
281  void publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr);
282 
283  /* -*- time sync -*- */
284 
285  inline void set_time_offset(uint64_t offset_ns) {
286  time_offset = offset_ns;
287  }
288 
289  inline uint64_t get_time_offset(void) {
290  return time_offset;
291  }
292 
293  inline void set_timesync_mode(timesync_mode mode) {
294  tsync_mode = mode;
295  }
296 
298  return tsync_mode;
299  }
300 
301  /* -*- autopilot version -*- */
302  uint64_t get_capabilities();
303  void update_capabilities(bool known, uint64_t caps = 0);
304 
312  ros::Time synchronise_stamp(uint32_t time_boot_ms);
313  ros::Time synchronise_stamp(uint64_t time_usec);
314 
324  template<typename T>
325  inline std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp) {
326  std_msgs::Header out;
327  out.frame_id = frame_id;
328  out.stamp = synchronise_stamp(time_stamp);
329  return out;
330  }
331 
332  /* -*- utils -*- */
333 
337  template<typename _T>
338  inline void msg_set_target(_T &msg) {
339  msg.target_system = get_tgt_system();
340  msg.target_component = get_tgt_component();
341  }
342 
346  inline bool is_my_target(uint8_t sysid, uint8_t compid) {
347  return sysid == get_tgt_system() && compid == get_tgt_component();
348  }
349 
353  inline bool is_my_target(uint8_t sysid) {
354  return sysid == get_tgt_system();
355  }
356 
360  inline bool is_ardupilotmega() {
361  return MAV_AUTOPILOT::ARDUPILOTMEGA == get_autopilot();
362  }
363 
367  inline bool is_px4() {
368  return MAV_AUTOPILOT::PX4 == get_autopilot();
369  }
370 
384  std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode);
385 
395  bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode);
396 
397 private:
398  std::recursive_mutex mutex;
399 
400  std::atomic<uint8_t> type;
401  std::atomic<uint8_t> autopilot;
402  std::atomic<uint8_t> base_mode;
403 
404  uint8_t target_system;
406 
407  std::atomic<bool> connected;
408  std::vector<ConnectionCb> connection_cb_vec;
409 
412 
414  float gps_eph;
415  float gps_epv;
418 
419  std::atomic<uint64_t> time_offset;
421 
422  std::atomic<bool> fcu_caps_known;
423  std::atomic<uint64_t> fcu_capabilities;
424 };
425 } // namespace mavros
bool is_connected()
Return connection status.
Definition: mavros_uas.h:95
std::atomic< uint8_t > autopilot
Definition: mavros_uas.h:401
uint8_t target_component
Definition: mavros_uas.h:405
bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode)
Lookup custom mode for given string.
mavconn::MAVConnInterface::Ptr fcu_link
MAVLink FCU device conection.
Definition: mavros_uas.h:80
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:325
sensor_msgs::NavSatFix::Ptr gps_fix
Definition: mavros_uas.h:413
double ellipsoid_to_geoid_height(T lla)
Conversion from height above ellipsoid (WGS-84) to height above geoid (AMSL)
Definition: mavros_uas.h:259
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:228
tf2_ros::TransformListener tf2_listener
Definition: mavros_uas.h:270
geometry_msgs::Quaternion get_attitude_orientation_enu()
Get Attitude orientation quaternion.
Definition: uas_data.cpp:139
uint64_t get_time_offset(void)
Definition: mavros_uas.h:289
MAV_AUTOPILOT get_autopilot()
Returns autopilot type.
Definition: mavros_uas.h:127
void update_connection_status(bool conn_)
Definition: uas_data.cpp:70
float gps_epv
Definition: mavros_uas.h:415
void set_time_offset(uint64_t offset_ns)
Definition: mavros_uas.h:285
diagnostic_updater::Updater diag_updater
Mavros diagnostic updater.
Definition: mavros_uas.h:90
sensor_msgs::Imu::Ptr imu_enu_data
Definition: mavros_uas.h:410
void msg_set_target(_T &msg)
Definition: mavros_uas.h:338
mavlink::common::MAV_TYPE MAV_TYPE
Definition: mavros_uas.h:73
mavlink::common::MAV_AUTOPILOT MAV_AUTOPILOT
Definition: mavros_uas.h:74
tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster
Definition: mavros_uas.h:272
bool get_hil_state()
Returns HIL status.
Definition: mavros_uas.h:145
std::function< void(bool)> ConnectionCb
Definition: mavros_uas.h:68
tf2_ros::TransformBroadcaster tf2_broadcaster
Definition: mavros_uas.h:271
geometry_msgs::Quaternion get_attitude_orientation_ned()
Get Attitude orientation quaternion.
Definition: uas_data.cpp:152
void update_capabilities(bool known, uint64_t caps=0)
Definition: uas_data.cpp:106
void update_attitude_imu_enu(sensor_msgs::Imu::Ptr &imu)
Store IMU data [ENU].
Definition: uas_data.cpp:115
uint8_t get_tgt_system()
Return communication target system.
Definition: mavros_uas.h:155
UAS for plugins.
Definition: mavros_uas.h:66
std::atomic< uint8_t > type
Definition: mavros_uas.h:400
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:269
std::shared_ptr< GeographicLib::Geoid > egm96_5
Geoid dataset used to convert between AMSL and WGS-84.
Definition: mavros_uas.h:239
void add_connection_change_handler(ConnectionCb cb)
Add connection change handler callback.
Definition: uas_data.cpp:81
bool is_ardupilotmega()
Check that FCU is APM.
Definition: mavros_uas.h:360
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:293
void set_tgt(uint8_t sys, uint8_t comp)
Definition: mavros_uas.h:166
geometry_msgs::Vector3 get_attitude_angular_velocity_ned()
Get angular velocity from IMU data.
Definition: uas_data.cpp:178
std::lock_guard< std::recursive_mutex > lock_guard
Definition: mavros_uas.h:69
std::atomic< bool > connected
Definition: mavros_uas.h:407
std::atomic< bool > fcu_caps_known
Definition: mavros_uas.h:422
bool get_armed()
Returns arming status.
Definition: mavros_uas.h:137
MAV_TYPE get_type()
Returns vehicle type.
Definition: mavros_uas.h:119
timesync_mode tsync_mode
Definition: mavros_uas.h:420
sensor_msgs::Imu::Ptr get_attitude_imu_ned()
Get IMU data [NED].
Definition: uas_data.cpp:133
bool is_my_target(uint8_t sysid, uint8_t compid)
Check that sys/comp id&#39;s is my target.
Definition: mavros_uas.h:346
std::recursive_mutex mutex
Definition: mavros_uas.h:398
void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_)
Definition: uas_data.cpp:63
std::unique_lock< std::recursive_mutex > unique_lock
Definition: mavros_uas.h:70
int gps_fix_type
Definition: mavros_uas.h:416
sensor_msgs::NavSatFix::Ptr get_gps_fix()
Retunrs last GPS RAW message.
Definition: uas_data.cpp:219
float gps_eph
Definition: mavros_uas.h:414
timesync_mode get_timesync_mode(void)
Definition: mavros_uas.h:297
uint64_t get_capabilities()
Definition: uas_data.cpp:95
mavlink::common::MAV_MODE_FLAG MAV_MODE_FLAG
Definition: mavros_uas.h:75
std::atomic< uint8_t > base_mode
Definition: mavros_uas.h:402
bool is_my_target(uint8_t sysid)
Check that system id is my target.
Definition: mavros_uas.h:353
uint8_t target_system
Definition: mavros_uas.h:404
sensor_msgs::Imu::Ptr imu_ned_data
Definition: mavros_uas.h:411
void update_attitude_imu_ned(sensor_msgs::Imu::Ptr &imu)
Store IMU data [NED].
Definition: uas_data.cpp:121
std::atomic< uint64_t > time_offset
Definition: mavros_uas.h:419
sensor_msgs::Imu::Ptr get_attitude_imu_enu()
Get IMU data [ENU].
Definition: uas_data.cpp:127
Frame transformation utilities.
bool is_px4()
Check that FCU is PX4.
Definition: mavros_uas.h:367
mavlink::common::MAV_STATE MAV_STATE
Definition: mavros_uas.h:76
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:194
std::atomic< uint64_t > fcu_capabilities
Definition: mavros_uas.h:423
uint8_t get_tgt_component()
Return communication target component.
Definition: mavros_uas.h:162
geometry_msgs::Vector3 get_attitude_angular_velocity_enu()
Get angular velocity from IMU data.
Definition: uas_data.cpp:165
double geoid_to_ellipsoid_height(T lla)
Conversion from height above geoid (AMSL) to height above ellipsoid (WGS-84)
Definition: mavros_uas.h:246
int gps_satellites_visible
Definition: mavros_uas.h:417
std::vector< ConnectionCb > connection_cb_vec
Definition: mavros_uas.h:408
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:208
std::shared_ptr< MAVConnInterface > Ptr


mavros
Author(s): Vladimir Ermakov
autogenerated on Mon Jul 8 2019 03:20:11