00001
00006
00007
00008
00009
00010
00011
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
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" },
00046 { 15, "GUIDED" },
00047 { 16, "INITIALISING" },
00048 { 17, "QSTABILIZE" },
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" },
00068 { 9, "LAND" },
00069 { 10, "OF_LOITER" },
00070 { 11, "DRIFT" },
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
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
00177 return str_custom_mode(custom_mode);
00178 }
00179
00180
00181
00182
00183
00184 static bool cmode_find_cmap(const cmode_map &cmap, std::string &cmode_str, uint32_t &cmode) {
00185
00186 for (auto &mode : cmap) {
00187 if (mode.second == cmode_str) {
00188 cmode = mode.first;
00189 return true;
00190 }
00191 }
00192
00193
00195 try {
00196 cmode = std::stoi(cmode_str, 0, 0);
00197 return true;
00198 }
00199 catch (std::invalid_argument &ex) {
00200
00201 }
00202
00203
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
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
00238
00240 static const std::array<const std::string, 18> autopilot_strings = {
00241 "Generic",
00242 "PIXHAWK",
00243 "SLUGS",
00244 "ArduPilotMega",
00245 "OpenPilot",
00246 "Generic-WP-Only",
00247 "Generic-WP-Simple-Nav",
00248 "Generic-Mission-Full",
00249 "INVALID",
00250 "Paparazzi",
00251 "UDB",
00252 "FlexiPilot",
00253 "PX4",
00254 "SMACCMPILOT",
00255 "AUTOQUAD",
00256 "ARMAZILA",
00257 "AEROB",
00258 "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 "Generic",
00273 "Fixed-Wing",
00274 "Quadrotor",
00275 "Coaxial-Heli",
00276 "Helicopter",
00277 "Antenna-Tracker",
00278 "GCS",
00279 "Airship",
00280 "Free-Balloon",
00281 "Rocket",
00282 "Ground-Rover",
00283 "Surface-Boat",
00284 "Submarine",
00285 "Hexarotor",
00286 "Octorotor",
00287 "Tricopter",
00288 "Flapping-Wing",
00289 "Kite",
00290 "Onboard-Controller",
00291 "VTOL-Duorotor",
00292 "VTOL-Quadrotor",
00293 "VTOL-Tiltrotor",
00294 "VTOL-RESERVED2",
00295 "VTOL-RESERVED3",
00296 "VTOL-RESERVED4",
00297 "VTOL-RESERVED5",
00298 "Gimbal",
00299 "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 "Uninit",
00313 "Boot",
00314 "Calibrating",
00315 "Standby",
00316 "Active",
00317 "Critical",
00318 "Emergency",
00319 "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