uas_stringify.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2014 Vladimir Ermakov.
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
14 #include <array>
15 #include <unordered_map>
16 #include <stdexcept>
17 #include <mavros/mavros_uas.h>
18 #include <mavros/px4_custom_mode.h>
19 
20 using namespace mavros;
22 
23 
24 /* -*- mode stringify functions -*- */
25 
26 typedef std::unordered_map<uint32_t, const std::string> cmode_map;
27 
33  { 0, "MANUAL" },
34  { 1, "CIRCLE" },
35  { 2, "STABILIZE" },
36  { 3, "TRAINING" },
37  { 4, "ACRO" },
38  { 5, "FBWA" },
39  { 6, "FBWB" },
40  { 7, "CRUISE" },
41  { 8, "AUTOTUNE" },
42  { 10, "AUTO" },
43  { 11, "RTL" },
44  { 12, "LOITER" },
45  { 14, "LAND" }, // not in list
46  { 15, "GUIDED" },
47  { 16, "INITIALISING" },
48  { 17, "QSTABILIZE" }, // QuadPlane
49  { 18, "QHOVER" },
50  { 19, "QLOITER" },
51  { 20, "QLAND" },
52  { 21, "QRTL" }
53 }};
54 
60  { 0, "STABILIZE" },
61  { 1, "ACRO" },
62  { 2, "ALT_HOLD" },
63  { 3, "AUTO" },
64  { 4, "GUIDED" },
65  { 5, "LOITER" },
66  { 6, "RTL" },
67  { 7, "CIRCLE" },
68  { 8, "POSITION" }, // not in list
69  { 9, "LAND" },
70  { 10, "OF_LOITER" },
71  { 11, "DRIFT" }, // renamed, prev name: APPROACH
72  { 13, "SPORT" },
73  { 14, "FLIP" },
74  { 15, "AUTOTUNE" },
75  { 16, "POSHOLD" },
76  { 17, "BRAKE" },
77  { 18, "THROW" },
78  { 19, "AVOID_ADSB" },
79  { 20, "GUIDED_NOGPS" }
80 }};
81 
87  { 0, "MANUAL" },
88  { 1, "ACRO" },
89  { 3, "STEERING" },
90  { 4, "HOLD" },
91  { 5, "LOITER" },
92  { 6, "FOLLOW" },
93  { 7, "SIMPLE" },
94  { 10, "AUTO" },
95  { 11, "RTL" },
96  { 12, "SMART_RTL" },
97  { 15, "GUIDED" },
98  { 16, "INITIALISING" }
99 }};
100 
108  { 0, "STABILIZE" },
109  { 1, "ACRO" },
110  { 2, "ALT_HOLD" },
111  { 3, "AUTO" }, // n/a
112  { 4, "GUIDED" }, // n/a
113  { 5, "VELHOLD" },
114  { 6, "RTL" }, // n/a
115  { 7, "CIRCLE" }, // n/a
116  { 9, "SURFACE" },
117  { 10, "OF_LOITER" }, // deprecated
118  { 11, "DRIFT" }, // n/a
119  { 13, "TRANSECT" },
120  { 14, "FLIP" }, // n/a
121  { 15, "AUTOTUNE" }, // n/a
122  { 16, "POSHOLD" },
123  { 17, "BRAKE" }, // n/a
124  { 18, "THROW" },
125  { 19, "MANUAL" }
126 }};
127 
129 static const cmode_map px4_cmode_map{{
146 }};
147 
148 static inline std::string str_base_mode(int base_mode) {
149  return utils::format("MODE(0x%2X)", base_mode);
150 }
151 
152 static std::string str_custom_mode(uint32_t custom_mode) {
153  return utils::format("CMODE(%u)", custom_mode);
154 }
155 
156 static std::string str_mode_cmap(const cmode_map &cmap, uint32_t custom_mode)
157 {
158  auto it = cmap.find(custom_mode);
159  if (it != cmap.end())
160  return it->second;
161  else
162  return str_custom_mode(custom_mode);
163 }
164 
165 static inline std::string str_mode_px4(uint32_t custom_mode_int)
166 {
167  px4::custom_mode custom_mode(custom_mode_int);
168 
169  // clear fields
170  custom_mode.reserved = 0;
171  if (custom_mode.main_mode != px4::custom_mode::MAIN_MODE_AUTO) {
172  ROS_WARN_COND_NAMED(custom_mode.sub_mode != 0, "uas", "PX4: Unknown sub-mode %d.%d",
173  custom_mode.main_mode, custom_mode.sub_mode);
174  custom_mode.sub_mode = 0;
175  }
176 
177  return str_mode_cmap(px4_cmode_map, custom_mode.data);
178 }
179 
180 static inline bool is_apm_copter(UAS::MAV_TYPE type)
181 {
182  return type == UAS::MAV_TYPE::QUADROTOR ||
183  type == UAS::MAV_TYPE::HEXAROTOR ||
184  type == UAS::MAV_TYPE::OCTOROTOR ||
185  type == UAS::MAV_TYPE::TRICOPTER ||
186  type == UAS::MAV_TYPE::COAXIAL;
187 }
188 
189 std::string UAS::str_mode_v10(uint8_t base_mode, uint32_t custom_mode)
190 {
191  if (!(base_mode & enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED)))
192  return str_base_mode(base_mode);
193 
194  auto type = get_type();
195  auto ap = get_autopilot();
196  if (MAV_AUTOPILOT::ARDUPILOTMEGA == ap) {
197  if (is_apm_copter(type))
198  return str_mode_cmap(arducopter_cmode_map, custom_mode);
199  else if (type == MAV_TYPE::FIXED_WING)
200  return str_mode_cmap(arduplane_cmode_map, custom_mode);
201  else if (type == MAV_TYPE::GROUND_ROVER)
202  return str_mode_cmap(apmrover2_cmode_map, custom_mode);
203  else if (type == MAV_TYPE::SURFACE_BOAT)
204  return str_mode_cmap(apmrover2_cmode_map, custom_mode); // NOTE: #1051 for now (19.06.2018) boat is same as rover
205  else if (type == MAV_TYPE::SUBMARINE)
206  return str_mode_cmap(ardusub_cmode_map, custom_mode);
207  else {
208  ROS_WARN_THROTTLE_NAMED(30, "uas", "MODE: Unknown APM based FCU! Type: %d", enum_value(type));
209  return str_custom_mode(custom_mode);
210  }
211  }
212  else if (MAV_AUTOPILOT::PX4 == ap)
213  return str_mode_px4(custom_mode);
214  else
215  /* TODO: other autopilot */
216  return str_custom_mode(custom_mode);
217 }
218 
219 /* XXX TODO
220  * Add a fallback CMODE(dec) decoder for unknown FCU's
221  */
222 
223 static bool cmode_find_cmap(const cmode_map &cmap, std::string &cmode_str, uint32_t &cmode)
224 {
225  // 1. try find by name
226  for (auto &mode : cmap) {
227  if (mode.second == cmode_str) {
228  cmode = mode.first;
229  return true;
230  }
231  }
232 
233  // 2. try convert integer
235  try {
236  cmode = std::stoi(cmode_str, 0, 0);
237  return true;
238  }
239  catch (std::invalid_argument &ex) {
240  // failed
241  }
242 
243  // Debugging output.
244  std::ostringstream os;
245  for (auto &mode : cmap)
246  os << " " << mode.second;
247 
248  ROS_ERROR_STREAM_NAMED("uas", "MODE: Unknown mode: " << cmode_str);
249  ROS_INFO_STREAM_NAMED("uas", "MODE: Known modes are:" << os.str());
250 
251  return false;
252 }
253 
254 bool UAS::cmode_from_str(std::string cmode_str, uint32_t &custom_mode)
255 {
256  // upper case
257  std::transform(cmode_str.begin(), cmode_str.end(), cmode_str.begin(), std::ref(toupper));
258 
259  auto type = get_type();
260  auto ap = get_autopilot();
261  if (MAV_AUTOPILOT::ARDUPILOTMEGA == ap) {
262  if (is_apm_copter(type))
263  return cmode_find_cmap(arducopter_cmode_map, cmode_str, custom_mode);
264  else if (type == MAV_TYPE::FIXED_WING)
265  return cmode_find_cmap(arduplane_cmode_map, cmode_str, custom_mode);
266  else if (type == MAV_TYPE::GROUND_ROVER)
267  return cmode_find_cmap(apmrover2_cmode_map, cmode_str, custom_mode);
268  else if (type == MAV_TYPE::SURFACE_BOAT)
269  return cmode_find_cmap(apmrover2_cmode_map, cmode_str, custom_mode);
270  else if (type == MAV_TYPE::SUBMARINE)
271  return cmode_find_cmap(ardusub_cmode_map, cmode_str, custom_mode);
272  }
273  else if (MAV_AUTOPILOT::PX4 == ap)
274  return cmode_find_cmap(px4_cmode_map, cmode_str, custom_mode);
275 
276  ROS_ERROR_NAMED("uas", "MODE: Unsupported FCU");
277  return false;
278 }
static std::string str_base_mode(int base_mode)
#define ROS_WARN_COND_NAMED(cond, name,...)
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
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
std::string format(const std::string &fmt, Args...args)
#define ROS_ERROR_STREAM_NAMED(name, args)
static const cmode_map apmrover2_cmode_map
MAV_AUTOPILOT get_autopilot()
Returns autopilot type.
Definition: mavros_uas.h:131
static std::string str_mode_px4(uint32_t custom_mode_int)
static bool is_apm_copter(UAS::MAV_TYPE type)
#define ROS_INFO_STREAM_NAMED(name, args)
static std::string str_custom_mode(uint32_t custom_mode)
static bool cmode_find_cmap(const cmode_map &cmap, std::string &cmode_str, uint32_t &cmode)
static const cmode_map ardusub_cmode_map
constexpr uint32_t define_mode_auto(enum custom_mode::SUB_MODE_AUTO sm)
helper function to define auto mode as uint32_t constant
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.
static const cmode_map arduplane_cmode_map
static std::string str_mode_cmap(const cmode_map &cmap, uint32_t custom_mode)
PX4 custom mode.
MAVROS Plugin context.
MAV_TYPE get_type()
Returns vehicle type.
Definition: mavros_uas.h:123
static const cmode_map arducopter_cmode_map
PX4 custom mode constantsPX4 custom flight modes.
#define ROS_ERROR_NAMED(name,...)
constexpr uint32_t define_mode(enum custom_mode::MAIN_MODE mm, uint8_t sm=0)
helper function to define any mode as uint32_t constant
std::unordered_map< uint32_t, const std::string > cmode_map
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
static const cmode_map px4_cmode_map
PX4 custom mode -> string.


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