VSM C++ SDK
Vehicle Specific Modules SDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
vehicle.h
Go to the documentation of this file.
1 // Copyright (c) 2018, Smart Projects Holdings Ltd
2 // All rights reserved.
3 // See LICENSE file for license details.
4 
11 #ifndef _UGCS_VSM_VEHICLE_H_
12 #define _UGCS_VSM_VEHICLE_H_
13 
16 #include <ugcs/vsm/mavlink.h>
17 #include <ugcs/vsm/enum_set.h>
18 #include <ugcs/vsm/device.h>
19 #include <ugcs/vsm/crc32.h>
20 
21 #include <stdint.h>
22 #include <memory>
23 #include <unordered_map>
24 #include <string>
25 #include <utility>
26 #include <vector>
27 
28 namespace ugcs {
29 namespace vsm {
30 
31 static constexpr int DEFAULT_COMMAND_TRY_COUNT = 3;
32 static constexpr std::chrono::milliseconds DEFAULT_COMMAND_TIMEOUT(1000);
33 
39 class Vehicle: public Device {
41 
42 public:
54  Vehicle(
55  proto::Device_type type = proto::DEVICE_TYPE_VEHICLE,
56  Request_processor::Ptr processor = nullptr,
57  Request_completion_context::Ptr completion_ctx = nullptr);
58 
60  virtual
61  ~Vehicle() {}
62 
64  Vehicle(const Vehicle &) = delete;
65 
71  void
73 
75  void
76  Set_serial_number(const std::string&);
77 
79  const std::string&
80  Get_serial_number() const;
81 
83  const std::string&
84  Get_model_name() const;
85 
87  void
88  Set_model_name(const std::string&);
89 
91  void
92  Set_port_name(const std::string&);
93 
95  const std::string&
96  Get_port_name() const;
97 
99  const std::string&
100  Get_frame_type() const;
101 
103  void
104  Set_frame_type(const std::string&);
105 
106  // Ground/Plane/Copter/VTOL...
107  proto::Vehicle_type
108  Get_vehicle_type() const;
109 
110  // Ground/Plane/Copter/VTOL...
111  void
112  Set_vehicle_type(proto::Vehicle_type);
113 
114  bool
115  Is_vehicle_type(proto::Vehicle_type type);
116 
117  bool
118  Is_copter() {
119  return vehicle_type == proto::VEHICLE_TYPE_HELICOPTER || vehicle_type == proto::VEHICLE_TYPE_MULTICOPTER;
120  }
121 
122 
125  class Hasher {
126  public:
128  size_t
129  operator()(const Vehicle::Ptr& ptr) const {
130  return hasher(ptr.get());
131  }
132  private:
134  static std::hash<Vehicle*> hasher;
135  };
136 
137 protected:
138  Optional<proto::Flight_mode> current_flight_mode;
139 
140  Property::Ptr t_control_mode = nullptr;
141  Property::Ptr t_is_armed = nullptr;
142  Property::Ptr t_uplink_present = nullptr;
143  Property::Ptr t_downlink_present = nullptr;
144  Property::Ptr t_main_voltage = nullptr;
145  Property::Ptr t_main_current = nullptr;
146  Property::Ptr t_gcs_link_quality = nullptr;
147  Property::Ptr t_rc_link_quality = nullptr;
148  Property::Ptr t_latitude = nullptr;
149  Property::Ptr t_longitude = nullptr;
150  Property::Ptr t_altitude_raw = nullptr;
151  Property::Ptr t_altitude_amsl = nullptr;
152  Property::Ptr t_altitude_agl = nullptr;
153  Property::Ptr t_ground_speed = nullptr;
154  Property::Ptr t_air_speed = nullptr;
155  Property::Ptr t_course = nullptr;
156  Property::Ptr t_vertical_speed = nullptr;
157  Property::Ptr t_pitch = nullptr;
158  Property::Ptr t_roll = nullptr;
159  Property::Ptr t_heading = nullptr;
160  Property::Ptr t_gps_fix = nullptr;
161  Property::Ptr t_satellite_count = nullptr;
162  Property::Ptr t_altitude_origin = nullptr;
163  Property::Ptr t_home_altitude_amsl = nullptr;
164  Property::Ptr t_home_altitude_raw = nullptr;
165  Property::Ptr t_home_latitude = nullptr;
166  Property::Ptr t_home_longitude = nullptr;
167  Property::Ptr t_target_altitude_amsl = nullptr;
168  Property::Ptr t_target_altitude_raw = nullptr;
169  Property::Ptr t_target_latitude = nullptr;
170  Property::Ptr t_target_longitude = nullptr;
171  Property::Ptr t_current_command = nullptr;
172  Property::Ptr t_current_mission_id = nullptr;
173  Property::Ptr t_flight_mode = nullptr;
174  Property::Ptr t_autopilot_status = nullptr;
175  Property::Ptr t_native_flight_mode = nullptr;
176  Property::Ptr t_fence_enabled = nullptr;
177  Property::Ptr t_vibration_x = nullptr;
178  Property::Ptr t_vibration_y = nullptr;
179  Property::Ptr t_vibration_z = nullptr;
180 
181  Vsm_command::Ptr c_mission_upload = nullptr;
182  Vsm_command::Ptr c_auto = nullptr;
183  Vsm_command::Ptr c_arm = nullptr;
184  Vsm_command::Ptr c_disarm = nullptr;
185  Vsm_command::Ptr c_waypoint = nullptr;
186  Vsm_command::Ptr c_guided = nullptr;
187  Vsm_command::Ptr c_manual = nullptr;
188  Vsm_command::Ptr c_pause = nullptr;
189  Vsm_command::Ptr c_resume = nullptr;
190  Vsm_command::Ptr c_rth = nullptr;
191  Vsm_command::Ptr c_land_command = nullptr;
192  Vsm_command::Ptr c_joystick = nullptr;
193  Vsm_command::Ptr c_direct_vehicle_control = nullptr;
194  Vsm_command::Ptr c_takeoff_command = nullptr;
195  Vsm_command::Ptr c_emergency_land = nullptr;
196  Vsm_command::Ptr c_camera_trigger_command = nullptr;
197  Vsm_command::Ptr c_adsb_set_ident = nullptr;
198  Vsm_command::Ptr c_adsb_set_mode = nullptr;
199  Vsm_command::Ptr c_direct_payload_control = nullptr;
200  Vsm_command::Ptr c_camera_power = nullptr;
201  Vsm_command::Ptr c_camera_video_source = nullptr;
202  Vsm_command::Ptr c_adsb_set_parameter = nullptr;
203  Vsm_command::Ptr c_set_servo = nullptr;
204  Vsm_command::Ptr c_repeat_servo = nullptr;
205  Vsm_command::Ptr c_set_fence = nullptr;
206  Vsm_command::Ptr c_trigger_calibration = nullptr;
207  Vsm_command::Ptr c_trigger_reboot = nullptr;
208  Vsm_command::Ptr c_get_native_route = nullptr;
209  Vsm_command::Ptr c_set_relative_heading = nullptr;
210  Vsm_command::Ptr c_write_parameter = nullptr;
211  Vsm_command::Ptr c_mission_clear = nullptr;
212 
213  Vsm_command::Ptr c_wait = nullptr;
214  Vsm_command::Ptr c_move = nullptr;
215  Vsm_command::Ptr c_set_speed = nullptr;
216  Vsm_command::Ptr c_set_home = nullptr;
217  Vsm_command::Ptr c_set_poi = nullptr;
218  Vsm_command::Ptr c_set_heading = nullptr;
219  Vsm_command::Ptr c_panorama = nullptr;
220  Vsm_command::Ptr c_camera_trigger_mission = nullptr;
221  Vsm_command::Ptr c_camera_by_time = nullptr;
222  Vsm_command::Ptr c_camera_by_distance = nullptr;
223  Vsm_command::Ptr c_land_mission = nullptr;
224  Vsm_command::Ptr c_takeoff_mission = nullptr;
225  Vsm_command::Ptr c_set_parameter = nullptr;
226  Vsm_command::Ptr c_payload_control = nullptr;
227  Vsm_command::Ptr c_transition_fixed = nullptr;
228  Vsm_command::Ptr c_transition_vtol = nullptr;
229  Vsm_command::Ptr c_wait_until = nullptr;
230 
231  Property::Ptr p_rc_loss_action = nullptr;
232  Property::Ptr p_gps_loss_action = nullptr;
233  Property::Ptr p_low_battery_action = nullptr;
234  Property::Ptr p_rth_action = nullptr;
235  Property::Ptr p_wp_turn_type = nullptr;
236 
237  int command_try_count = DEFAULT_COMMAND_TRY_COUNT;
238  std::chrono::milliseconds command_timeout = DEFAULT_COMMAND_TIMEOUT;
239 
240  // Predefined subsytems
241  Subsystem::Ptr flight_controller;
242  Subsystem::Ptr primary_camera;
243  Subsystem::Ptr primary_gimbal;
244  Subsystem::Ptr adsb_transponder;
245  Subsystem::Ptr winch_controller;
246 
250  virtual void
252  {};
253 
257  virtual void
259  {};
260 
261  // Sets available FS actions on RC loss with the first one as default.
262  void
263  Set_rc_loss_actions(std::initializer_list<proto::Failsafe_action> actions);
264 
265  // Sets available FS actions on GPS loss with the first one as default.
266  void
267  Set_gps_loss_actions(std::initializer_list<proto::Failsafe_action> actions);
268 
269  // Sets available FS actions on low battery with the first one as default.
270  void
271  Set_low_battery_actions(std::initializer_list<proto::Failsafe_action> actions);
272 
273  // Sets available actions after RTH with the first one as default.
274  void
275  Set_rth_actions(std::initializer_list<proto::Rth_action> actions);
276 
283  void
285 
289  void
290  Set_altitude_origin(float altitude_amsl);
291 
292  /*
293  * Below are methods which are called by VSM SDK in vehicle context and
294  * should be overridden by user code.
295  */
296 
301  // Translates incoming message to old style Vehicle_request
302  virtual void
304  Ucs_request::Ptr ucs_request);
305 
306  // old style completion handler
307  void
308  Command_completed(
310  const std::string& status_text,
311  Ucs_request::Ptr ucs_request);
312 
313  void
314  Command_failed(
315  Ucs_request::Ptr ucs_request,
316  const std::string& status_text,
317  proto::Status_code code = proto::STATUS_FAILED);
318 
319  void
320  Command_succeeded(
321  Ucs_request::Ptr ucs_request);
322 
326  virtual void
328 
332  virtual void
334 
335  /* End of methods called by VSM SDK. */
336 
337  /* Below are methods which should be called by user code from derived
338  * vehicle class. */
339 
340  // Convenience function to check current flight mode.
341  bool
342  Is_flight_mode(proto::Flight_mode);
343 
344  // Return true if vehicle currently is in given control mode m
345  bool
346  Is_control_mode(proto::Control_mode m);
347 
348  // Mission command mapping interface. Used to support current command reporting
349  // During mission flight.
350  //
351  // Here is how it works:
352  // 1. mission_upload command contains a list of sub_commands. VSM enumerates it as zero-based.
353  // This becomes the mission_command_id
354  // 2. Each mission command can produce any number of vehicle specific commands.
355  // 3. The mapping is sent back to server as a response to mission_upload command together with generated mission_id.
356  // 4. VSM reports current_vehicle_command to the server during mission flight.
357  // 5. Server uses this mapping maps vehicle_specific commands back to mission_commands.
358  // 6. mission_id is used to synchronize the command mapping with the server when
359  // the mapping is unknown to VSM (eg. after restart).
360  // 7. Server uses the command map only if the map exists and the reported mission_id in telemetry is equal to the
361  // mission_id received together with the map.
362  // 8. If server receives different mission_id it drops the current mapping.
363  class Command_map {
364  public:
365  Command_map() : mission_id() {}
366 
367  // Clear the command mapping and reset mission_id.
368  void
369  Reset();
370 
371  // Set the current_mission_command id as received from ucs.
372  // All vehicle specific commands produced by this mission command
373  // will map to this ID.
374  void
375  Set_current_command(int mission_command_id);
376 
377  // Map current_mission_command to this vehicle_command_id
378  // VSM must call this on each mission item it uploads to the vehicle.
379  // Or more precisely: on each command the vehicle is going to report as current command.
380  void
381  Add_command_mapping(int vehicle_specific_id);
382 
383  // Use this function to build mission_id.
384  // Used in two scenarios:
385  // 1) To report newly uploaded mission_id to server.
386  // 2) To calculate mission_id from downloaded mission from the vehicle.
387  // And report as telemetry when VSM has not seen the mission_upload.
388  // CRC32 algorithm is used to create a 32bit hash.
389  // IMPORTANT:
390  // Mission ID is calculated as hash from uploaded vehicle commands
391  // in such way that it can be recreated if vehicle supports mission
392  // download.
393  void
394  Accumulate_route_id(uint32_t hash);
395 
396  // Get the generated mission_id.
397  uint32_t
398  Get_route_id();
399 
400  // Add additional value to mission id hash which can be changed without regenerating the
401  // mission_command_map. Used to add Home Location to route hash.
402  void
403  Set_secondary_id(uint32_t);
404 
405  // Fill in the mission_upload response payload with accumulated map and mission_id.
406  void
407  Fill_command_mapping_response(Proto_msg_ptr response);
408 
409  private:
410  // Mission mapper state.
411  int current_mission_command = -1;
412  // Current native command mapping to mission subcommands (zero based).
413  std::unordered_map<int, int> mission_command_map;
414 
415  uint32_t secondary_id = 0;
416 
417  Crc32 mission_id;
418  };
419 
420  /* End of user callable methods. */
421 
422  void
423  Set_autopilot_type(const std::string&);
424 
425  void
426  Set_autopilot_serial(const std::string&);
427 
428 private:
429 
433  template<class Request_ptr>
434  void
435  Submit_vehicle_request(Request_ptr vehicle_request)
436  {
437  using Request = typename Request_ptr::element_type;
438  using Handle = typename Request::Handle;
439  typedef void (Vehicle::*Handler)(Handle);
440 
441  Handler method = &Vehicle::Handle_vehicle_request;
442  auto proc_handler = Make_callback(method,
443  Shared_from_this(),
444  Handle(vehicle_request));
445  vehicle_request->request->Set_processing_handler(proc_handler);
446  processor->Submit_request(vehicle_request->request);
447  }
448 
450  Optional<float> current_altitude_origin;
451 
452  /* Friend classes mostly for accessing system_id variable which we want
453  * to hide from SDK user.
454  */
455  friend class Ucs_vehicle_ctx;
456  friend class Ucs_transaction;
457  friend class Ucs_mission_clear_all_transaction;
458  friend class Ucs_task_upload_transaction;
459  friend class Ucs_vehicle_command_transaction;
460  friend class Cucs_processor;
461 
463  proto::Vehicle_type vehicle_type = proto::VEHICLE_TYPE_MULTICOPTER;
464 
466  std::string serial_number;
467 
469  std::string model_name;
470 
472  std::string port_name;
473 
475  std::string frame_type;
476 
479  std::string vehicle_serial_prefix;
480 };
481 
484 #define VEHICLE_LOG(level_, vehicle_, fmt_, ...) \
485  _LOG_WRITE_MSG(level_, "[%s:%s] " fmt_, \
486  (vehicle_).Get_model_name().c_str(), \
487  (vehicle_).Get_serial_number().c_str(), ## __VA_ARGS__)
488 
491 // @{
492 #define VEHICLE_LOG_DBG(vehicle_, fmt_, ...) \
493  VEHICLE_LOG(::ugcs::vsm::Log::Level::DEBUGGING, vehicle_, fmt_, ## __VA_ARGS__)
494 
495 #define VEHICLE_LOG_INF(vehicle_, fmt_, ...) \
496  VEHICLE_LOG(::ugcs::vsm::Log::Level::INFO, vehicle_, fmt_, ## __VA_ARGS__)
497 
498 #define VEHICLE_LOG_WRN(vehicle_, fmt_, ...) \
499  VEHICLE_LOG(::ugcs::vsm::Log::Level::WARNING, vehicle_, fmt_, ## __VA_ARGS__)
500 
501 #define VEHICLE_LOG_ERR(vehicle_, fmt_, ...) \
502  VEHICLE_LOG(::ugcs::vsm::Log::Level::ERROR, vehicle_, fmt_, ## __VA_ARGS__)
503 // @}
504 
505 } /* namespace vsm */
506 } /* namespace ugcs */
507 
508 #endif /* _UGCS_VSM_VEHICLE_H_ */
Generic request for implementing inter-threads communications and asynchronous operations.
Definition: request_container.h:37
Request worker.
std::shared_ptr< Property > Ptr
Pointer type.
Definition: property.h:18
void Set_frame_type(const std::string &)
Set frame type of the vehicle.
Definition: crc32.h:14
virtual void On_disable()
Vehicle disable event handler.
Definition: vehicle.h:258
Definition: vehicle.h:363
Hasher for Vehicle shared pointer.
Definition: vehicle.h:125
void Set_port_name(const std::string &)
Set port name.
size_t operator()(const Vehicle::Ptr &ptr) const
Get hash value.
Definition: vehicle.h:129
virtual void On_enable()
Vehicle enable event handler.
Definition: vehicle.h:251
const std::string & Get_model_name() const
Get model name of the vehicle.
void Set_altitude_origin(float altitude_amsl)
Tell server that Vehicle knows its altitude_origin.
Request execution context.
Definition: request_context.h:24
void Set_model_name(const std::string &)
Set model name of the vehicle.
const std::string & Get_port_name() const
Get port name the vehicle is connected to.
Result
Request completion result.
Definition: vehicle_request.h:26
std::shared_ptr< Vehicle > Ptr
Pointer type.
Definition: vehicle.h:40
Callback< Callable, void, Args...>::Ptr Make_callback(Callable &&callable, Args &&...args)
Create a callback.
Definition: callback.h:389
Vehicle(proto::Device_type type=proto::DEVICE_TYPE_VEHICLE, Request_processor::Ptr processor=nullptr, Request_completion_context::Ptr completion_ctx=nullptr)
Constructor for a base class.
Handle of a specific vehicle request.
Definition: vehicle_request.h:156
Handles interactions with CUCS.
Definition: cucs_processor.h:34
Definition: device.h:44
std::shared_ptr< Vsm_command > Ptr
Pointer type.
Definition: subsystem.h:24
virtual void Handle_ucs_command(Ucs_request::Ptr ucs_request)
Message from ucs arrived.
std::shared_ptr< Device > Ptr
Pointer type.
Definition: device.h:46
const std::string & Get_serial_number() const
Get serial number of the vehicle.
void Process_requests()
Process requests assigned to vehicle in user thread, i.e.
virtual void Handle_vehicle_request(Vehicle_task_request::Handle request)
Task has arrived from UCS and should be uploaded to the vehicle.
std::shared_ptr< Subsystem > Ptr
Pointer type.
Definition: subsystem.h:84
#define DEFINE_COMMON_CLASS(__class_name,...)
Use this macro to define some common attributes for a class.
Definition: utils.h:25
void Reset_altitude_origin()
Tell server that current altitude origin must be dropped.
Convenience types for all specific vehicle requests.
void Set_serial_number(const std::string &)
Set serial number.
std::shared_ptr< Ucs_request > Ptr
Pointer type.
Definition: device.h:25
const std::string & Get_frame_type() const
Get frame type of the vehicle.
Base class for user-defined vehicles.
Definition: vehicle.h:39