uas_stringify.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2014 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 
00024 /* -*- mode stringify functions -*- */
00025 
00026 typedef std::unordered_map<uint32_t, const std::string> cmode_map;
00027 
00032 static const cmode_map arduplane_cmode_map = {
00033         { 0, "MANUAL" },
00034         { 1, "CIRCLE" },
00035         { 2, "STABILIZE" },
00036         { 3, "TRAINING" },
00037         { 4, "ACRO" },
00038         { 5, "FBWA" },
00039         { 6, "FBWB" },
00040         { 7, "CRUISE" },
00041         { 8, "AUTOTUNE" },
00042         { 10, "AUTO" },
00043         { 11, "RTL" },
00044         { 12, "LOITER" },
00045         { 14, "LAND" },         // not in list
00046         { 15, "GUIDED" },
00047         { 16, "INITIALISING" },
00048         { 17, "QSTABILIZE" },   // QuadPlane
00049         { 18, "QHOVER" },
00050         { 19, "QLOITER" },
00051         { 20, "QLAND" }
00052 };
00053 
00058 static const cmode_map arducopter_cmode_map = {
00059         { 0, "STABILIZE" },
00060         { 1, "ACRO" },
00061         { 2, "ALT_HOLD" },
00062         { 3, "AUTO" },
00063         { 4, "GUIDED" },
00064         { 5, "LOITER" },
00065         { 6, "RTL" },
00066         { 7, "CIRCLE" },
00067         { 8, "POSITION" },      // not in list
00068         { 9, "LAND" },
00069         { 10, "OF_LOITER" },
00070         { 11, "DRIFT" },        // renamed, prev name: APPROACH
00071         { 13, "SPORT" },
00072         { 14, "FLIP" },
00073         { 15, "AUTOTUNE" },
00074         { 16, "POSHOLD" },
00075         { 17, "BRAKE" }
00076 };
00077 
00082 static const cmode_map apmrover2_cmode_map = {
00083         { 0, "MANUAL" },
00084         { 2, "LEARNING" },
00085         { 3, "STEERING" },
00086         { 4, "HOLD" },
00087         { 10, "AUTO" },
00088         { 11, "RTL" },
00089         { 15, "GUIDED" },
00090         { 16, "INITIALISING" }
00091 };
00092 
00094 static const cmode_map px4_cmode_map = {
00095         { px4::define_mode(px4::custom_mode::MAIN_MODE_MANUAL),           "MANUAL" },
00096         { px4::define_mode(px4::custom_mode::MAIN_MODE_ACRO),             "ACRO" },
00097         { px4::define_mode(px4::custom_mode::MAIN_MODE_ALTCTL),           "ALTCTL" },
00098         { px4::define_mode(px4::custom_mode::MAIN_MODE_POSCTL),           "POSCTL" },
00099         { px4::define_mode(px4::custom_mode::MAIN_MODE_OFFBOARD),         "OFFBOARD" },
00100         { px4::define_mode(px4::custom_mode::MAIN_MODE_STABILIZED),       "STABILIZED" },
00101         { px4::define_mode(px4::custom_mode::MAIN_MODE_RATTITUDE),        "RATTITUDE" },
00102         { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_MISSION), "AUTO.MISSION" },
00103         { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LOITER),  "AUTO.LOITER" },
00104         { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTL),     "AUTO.RTL" },
00105         { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LAND),    "AUTO.LAND" },
00106         { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTGS),    "AUTO.RTGS" },
00107         { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_READY),   "AUTO.READY" },
00108         { px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_TAKEOFF), "AUTO.TAKEOFF" }
00109 };
00110 
00111 static inline std::string str_base_mode(int base_mode) {
00112         std::ostringstream mode;
00113         mode << "MODE(0x" << std::hex << std::uppercase << base_mode << ")";
00114         return mode.str();
00115 }
00116 
00117 static std::string str_custom_mode(uint32_t custom_mode) {
00118         std::ostringstream mode;
00119         mode << "CMODE(" << custom_mode << ")";
00120         return mode.str();
00121 }
00122 
00123 static std::string str_mode_cmap(const cmode_map &cmap, uint32_t custom_mode) {
00124         auto it = cmap.find(custom_mode);
00125         if (it != cmap.end())
00126                 return it->second;
00127         else
00128                 return str_custom_mode(custom_mode);
00129 }
00130 
00131 static inline std::string str_mode_px4(uint32_t custom_mode_int) {
00132         px4::custom_mode custom_mode(custom_mode_int);
00133 
00134         // clear fields
00135         custom_mode.reserved = 0;
00136         if (custom_mode.main_mode != px4::custom_mode::MAIN_MODE_AUTO) {
00137                 ROS_WARN_COND_NAMED(custom_mode.sub_mode != 0, "uas", "PX4: Unknown sub-mode %d.%d",
00138                                 custom_mode.main_mode, custom_mode.sub_mode);
00139                 custom_mode.sub_mode = 0;
00140         }
00141 
00142         return str_mode_cmap(px4_cmode_map, custom_mode.data);
00143 }
00144 
00145 static inline bool is_apm_copter(enum MAV_TYPE &type) {
00146         return type == MAV_TYPE_QUADROTOR ||
00147                type == MAV_TYPE_HEXAROTOR ||
00148                type == MAV_TYPE_OCTOROTOR ||
00149                type == MAV_TYPE_TRICOPTER ||
00150                type == MAV_TYPE_COAXIAL;
00151 }
00152 
00153 std::string UAS::str_mode_v10(uint8_t base_mode, uint32_t custom_mode) {
00154         if (!(base_mode && MAV_MODE_FLAG_CUSTOM_MODE_ENABLED))
00155                 return str_base_mode(base_mode);
00156 
00157         auto type = get_type();
00158         auto ap = get_autopilot();
00159         if (MAV_AUTOPILOT_ARDUPILOTMEGA == ap) {
00160                 if (is_apm_copter(type))
00161                         return str_mode_cmap(arducopter_cmode_map, custom_mode);
00162                 else if (type == MAV_TYPE_FIXED_WING)
00163                         return str_mode_cmap(arduplane_cmode_map, custom_mode);
00164                 else if (type == MAV_TYPE_GROUND_ROVER)
00165                         return str_mode_cmap(apmrover2_cmode_map, custom_mode);
00166                 else if (type == MAV_TYPE_SUBMARINE)
00167                         return str_mode_cmap(arducopter_cmode_map, custom_mode);
00168                 else {
00169                         ROS_WARN_THROTTLE_NAMED(30, "uas", "MODE: Unknown APM based FCU! Type: %d", type);
00170                         return str_custom_mode(custom_mode);
00171                 }
00172         }
00173         else if (MAV_AUTOPILOT_PX4 == ap)
00174                 return str_mode_px4(custom_mode);
00175         else
00176                 /* TODO: other autopilot */
00177                 return str_custom_mode(custom_mode);
00178 }
00179 
00180 /* XXX TODO
00181  * Add a fallback CMODE(dec) decoder for unknown FCU's
00182  */
00183 
00184 static bool cmode_find_cmap(const cmode_map &cmap, std::string &cmode_str, uint32_t &cmode) {
00185         // 1. try find by name
00186         for (auto &mode : cmap) {
00187                 if (mode.second == cmode_str) {
00188                         cmode = mode.first;
00189                         return true;
00190                 }
00191         }
00192 
00193         // 2. try convert integer
00195         try {
00196                 cmode = std::stoi(cmode_str, 0, 0);
00197                 return true;
00198         }
00199         catch (std::invalid_argument &ex) {
00200                 // failed
00201         }
00202 
00203         // Debugging output.
00204         std::ostringstream os;
00205         for (auto &mode : cmap)
00206                 os << " " << mode.second;
00207 
00208         ROS_ERROR_STREAM_NAMED("uas", "MODE: Unknown mode: " << cmode_str);
00209         ROS_INFO_STREAM_NAMED("uas", "MODE: Known modes are:" << os.str());
00210 
00211         return false;
00212 }
00213 
00214 bool UAS::cmode_from_str(std::string cmode_str, uint32_t &custom_mode) {
00215         // upper case
00216         std::transform(cmode_str.begin(), cmode_str.end(), cmode_str.begin(), std::ref(toupper));
00217 
00218         auto type = get_type();
00219         auto ap = get_autopilot();
00220         if (MAV_AUTOPILOT_ARDUPILOTMEGA == ap) {
00221                 if (is_apm_copter(type))
00222                         return cmode_find_cmap(arducopter_cmode_map, cmode_str, custom_mode);
00223                 else if (type == MAV_TYPE_FIXED_WING)
00224                         return cmode_find_cmap(arduplane_cmode_map, cmode_str, custom_mode);
00225                 else if (type == MAV_TYPE_GROUND_ROVER)
00226                         return cmode_find_cmap(apmrover2_cmode_map, cmode_str, custom_mode);
00227                 else if (type == MAV_TYPE_SUBMARINE)
00228                         return cmode_find_cmap(arducopter_cmode_map, cmode_str, custom_mode);
00229         }
00230         else if (MAV_AUTOPILOT_PX4 == ap)
00231                 return cmode_find_cmap(px4_cmode_map, cmode_str, custom_mode);
00232 
00233         ROS_ERROR_NAMED("uas", "MODE: Unsupported FCU");
00234         return false;
00235 }
00236 
00237 /* -*- enum stringify -*- */
00238 
00240 static const std::array<const std::string, 18> autopilot_strings = {
00241         /*  0 */ "Generic",
00242         /*  1 */ "PIXHAWK",
00243         /*  2 */ "SLUGS",
00244         /*  3 */ "ArduPilotMega",
00245         /*  4 */ "OpenPilot",
00246         /*  5 */ "Generic-WP-Only",
00247         /*  6 */ "Generic-WP-Simple-Nav",
00248         /*  7 */ "Generic-Mission-Full",
00249         /*  8 */ "INVALID",
00250         /*  9 */ "Paparazzi",
00251         /* 10 */ "UDB",
00252         /* 11 */ "FlexiPilot",
00253         /* 12 */ "PX4",
00254         /* 13 */ "SMACCMPILOT",
00255         /* 14 */ "AUTOQUAD",
00256         /* 15 */ "ARMAZILA",
00257         /* 16 */ "AEROB",
00258         /* 17 */ "ASLUAV"
00259 };
00260 
00261 std::string UAS::str_autopilot(enum MAV_AUTOPILOT ap)
00262 {
00263         size_t idx = size_t(ap);
00264         if (idx >= autopilot_strings.size())
00265                 return std::to_string(idx);
00266 
00267         return autopilot_strings[idx];
00268 }
00269 
00271 static const std::array<const std::string, 28> type_strings = {
00272         /*  0 */ "Generic",
00273         /*  1 */ "Fixed-Wing",
00274         /*  2 */ "Quadrotor",
00275         /*  3 */ "Coaxial-Heli",
00276         /*  4 */ "Helicopter",
00277         /*  5 */ "Antenna-Tracker",
00278         /*  6 */ "GCS",
00279         /*  7 */ "Airship",
00280         /*  8 */ "Free-Balloon",
00281         /*  9 */ "Rocket",
00282         /* 10 */ "Ground-Rover",
00283         /* 11 */ "Surface-Boat",
00284         /* 12 */ "Submarine",
00285         /* 13 */ "Hexarotor",
00286         /* 14 */ "Octorotor",
00287         /* 15 */ "Tricopter",
00288         /* 16 */ "Flapping-Wing",
00289         /* 17 */ "Kite",
00290         /* 18 */ "Onboard-Controller",
00291         /* 19 */ "VTOL-Duorotor",
00292         /* 20 */ "VTOL-Quadrotor",
00293         /* 21 */ "VTOL-Tiltrotor",
00294         /* 22 */ "VTOL-RESERVED2",
00295         /* 23 */ "VTOL-RESERVED3",
00296         /* 24 */ "VTOL-RESERVED4",
00297         /* 25 */ "VTOL-RESERVED5",
00298         /* 26 */ "Gimbal",
00299         /* 27 */ "ADS-B"
00300 };
00301 
00302 std::string UAS::str_type(enum MAV_TYPE type)
00303 {
00304         size_t idx = size_t(type);
00305         if (idx >= type_strings.size())
00306                 return std::to_string(idx);
00307 
00308         return type_strings[idx];
00309 }
00310 
00311 static const std::array<const std::string, 8> state_strings = {
00312         /*  0 */ "Uninit",
00313         /*  1 */ "Boot",
00314         /*  2 */ "Calibrating",
00315         /*  3 */ "Standby",
00316         /*  4 */ "Active",
00317         /*  5 */ "Critical",
00318         /*  6 */ "Emergency",
00319         /*  7 */ "Poweroff"
00320 };
00321 
00322 std::string UAS::str_system_status(enum MAV_STATE st)
00323 {
00324         size_t idx = size_t(st);
00325         if (idx >= state_strings.size())
00326                 return std::to_string(idx);
00327 
00328         return state_strings[idx];
00329 }
00330 


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