uas_data.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2014,2015 Vladimir Ermakov.
00008  *
00009  * This file is part of the mavros package and subject to the license terms
00010  * in the top-level LICENSE file of the mavros repository.
00011  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00012  */
00013 
00014 #include <array>
00015 #include <unordered_map>
00016 #include <stdexcept>
00017 #include <mavros/mavros_uas.h>
00018 #include <mavros/utils.h>
00019 #include <mavros/px4_custom_mode.h>
00020 
00021 using namespace mavros;
00022 
00023 UAS::UAS() :
00024         tf2_listener(tf2_buffer, true),
00025         type(MAV_TYPE_GENERIC),
00026         autopilot(MAV_AUTOPILOT_GENERIC),
00027         base_mode(0),
00028         target_system(1),
00029         target_component(1),
00030         connected(false),
00031         gps_eph(NAN),
00032         gps_epv(NAN),
00033         gps_fix_type(0),
00034         gps_satellites_visible(0),
00035         time_offset(0),
00036         fcu_caps_known(false),
00037         fcu_capabilities(0)
00038 {}
00039 
00040 void UAS::stop(void)
00041 {}
00042 
00043 
00044 /* -*- heartbeat handlers -*- */
00045 
00046 void UAS::update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_)
00047 {
00048         type = type_;
00049         autopilot = autopilot_;
00050         base_mode = base_mode_;
00051 }
00052 
00053 void UAS::update_connection_status(bool conn_)
00054 {
00055         if (conn_ != connected) {
00056                 connected = conn_;
00057                 sig_connection_changed(connected);
00058         }
00059 }
00060 
00061 
00062 /* -*- autopilot version -*- */
00063 
00064 static uint64_t get_default_caps(uint8_t ap_type)
00065 {
00066         // TODO: return default caps mask for known FCU's
00067         return 0;
00068 }
00069 
00070 uint64_t UAS::get_capabilities()
00071 {
00072         if (fcu_caps_known) {
00073                 uint64_t caps = fcu_capabilities;
00074                 return caps;
00075         }
00076         else {
00077                 return get_default_caps(get_autopilot());
00078         }
00079 }
00080 
00081 void UAS::update_capabilities(bool known, uint64_t caps)
00082 {
00083         fcu_caps_known = known;
00084         fcu_capabilities = caps;
00085 }
00086 
00087 
00088 /* -*- IMU data -*- */
00089 
00090 void UAS::update_attitude_imu(sensor_msgs::Imu::Ptr &imu)
00091 {
00092         lock_guard lock(mutex);
00093         imu_data = imu;
00094 }
00095 
00096 sensor_msgs::Imu::Ptr UAS::get_attitude_imu()
00097 {
00098         lock_guard lock(mutex);
00099         return imu_data;
00100 }
00101 
00102 geometry_msgs::Quaternion UAS::get_attitude_orientation()
00103 {
00104         lock_guard lock(mutex);
00105         if (imu_data)
00106                 return imu_data->orientation;
00107         else {
00108                 // fallback - return identity
00109                 geometry_msgs::Quaternion q;
00110                 q.w = 1.0; q.x = q.y = q.z = 0.0;
00111                 return q;
00112         }
00113 }
00114 
00115 geometry_msgs::Vector3 UAS::get_attitude_angular_velocity()
00116 {
00117         lock_guard lock(mutex);
00118         if (imu_data)
00119                 return imu_data->angular_velocity;
00120         else {
00121                 // fallback
00122                 geometry_msgs::Vector3 v;
00123                 v.x = v.y = v.z = 0.0;
00124                 return v;
00125         }
00126 }
00127 
00128 
00129 /* -*- GPS data -*- */
00130 
00131 void UAS::update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix,
00132                 float eph, float epv,
00133                 int fix_type, int satellites_visible)
00134 {
00135         lock_guard lock(mutex);
00136 
00137         gps_fix = fix;
00138         gps_eph = eph;
00139         gps_epv = epv;
00140         gps_fix_type = fix_type;
00141         gps_satellites_visible = satellites_visible;
00142 }
00143 
00145 void UAS::get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible)
00146 {
00147         lock_guard lock(mutex);
00148 
00149         eph = gps_eph;
00150         epv = gps_epv;
00151         fix_type = gps_fix_type;
00152         satellites_visible = gps_satellites_visible;
00153 }
00154 
00156 sensor_msgs::NavSatFix::Ptr UAS::get_gps_fix()
00157 {
00158         lock_guard lock(mutex);
00159         return gps_fix;
00160 }
00161 


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19