VSM C++ SDK
Vehicle Specific Modules SDK
move_action.h
Go to the documentation of this file.
1 // Copyright (c) 2014, Smart Projects Holdings Ltd
2 // All rights reserved.
3 // See LICENSE file for license details.
4 
10 #ifndef _MOVE_ACTION_H_
11 #define _MOVE_ACTION_H_
12 
13 #include <ugcs/vsm/action.h>
14 #include <ugcs/vsm/coordinates.h>
15 #include <ugcs/vsm/mavlink.h>
16 #include <ugcs/vsm/optional.h>
17 #include <ugcs/vsm/property.h>
18 
19 namespace ugcs {
20 namespace vsm {
21 
23 class Move_action: public Action {
25 
26 public:
27 
31  typedef enum {
32  TURN_TYPE_STOP_AND_TURN = 0,
33  TURN_TYPE_STRAIGHT = 1,
34  TURN_TYPE_SPLINE = 2,
35  TURN_TYPE_BANK_TURN = 3,
36  } Turn_type;
37 
40  double loiter_orbit, double heading, double elevation) :
41  Action(Type::MOVE),
42  position(position),
43  wait_time(wait_time),
44  acceptance_radius(acceptance_radius),
45  loiter_orbit(loiter_orbit),
46  heading(heading),
47  elevation(elevation),
48  turn_type(TURN_TYPE_STRAIGHT)
49  {}
50 
56  Move_action(const mavlink::ugcs::Pld_mission_item_ex& item, Optional<double>& takeoff_altitude) :
57  Action(Type::MOVE),
58  position(Geodetic_tuple(item->x * M_PI / 180.0,
59  item->y * M_PI / 180.0,
60  item->z)),
61  wait_time(0.1 * item->param1),
62  acceptance_radius(item->param2),
63  loiter_orbit(item->param3),
64  heading(item->param4 * M_PI / 180.0),
65  elevation(item->elevation)
66  {
67  ASSERT(item->command == mavlink::MAV_CMD::MAV_CMD_NAV_WAYPOINT);
68  if (item->altitude_origin.Is_reset()) {
69  takeoff_altitude = item->elevation;
70  } else {
71  takeoff_altitude = item->altitude_origin;
72  }
73  turn_type = static_cast<Turn_type>(item->turn_type.Get());
74  }
75 
80  Action(Type::MOVE),
81  position(Geodetic_tuple(0,0,0))
82  {
83  double lat = 0, lon = 0, alt = 0;
84  auto pi = p.find("latitude");
85  if (pi != p.end()) {
86  pi->second->Get_value(lat);
87  }
88  pi = p.find("longitude");
89  if (pi != p.end()) {
90  pi->second->Get_value(lon);
91  }
92  pi = p.find("altitude_amsl");
93  if (pi != p.end()) {
94  pi->second->Get_value(alt);
95  }
96  position = Geodetic_tuple(lat, lon, alt);
97  p.at("acceptance_radius")->Get_value(acceptance_radius);
98  p.at("heading")->Get_value(heading);
99  p.at("loiter_radius")->Get_value(loiter_orbit);
100  p.at("wait_time")->Get_value(wait_time);
101  p.at("ground_elevation")->Get_value(elevation);
102  int t;
103  p.at("turn_type")->Get_value(t);
104  turn_type = static_cast<Turn_type>(t);
105  }
106 
111 
116  double wait_time = 0;
117 
123  double acceptance_radius = 0;
124 
130  double loiter_orbit = 0;
131 
135  double heading = 0;
136 
140  double elevation = 0;
141 
142  Turn_type turn_type;
143 };
144 
146 template<>
149  typedef Move_action type;
150 };
151 
152 } /* namespace vsm */
153 } /* namespace ugcs */
154 
155 #endif /* _MOVE_ACTION_H_ */
UGCS root namespace.
Definition: android-linux/ugcs/vsm/platform_sockets.h:27
Move action ugcs::vsm::Move_action.
Definition: property.h:191
double elevation
Elevation in meters (i.e.
Definition: move_action.h:140
Move_action type
Real type.
Definition: move_action.h:149
Move_action(const mavlink::ugcs::Pld_mission_item_ex &item, Optional< double > &takeoff_altitude)
Construct move action from Mavlink extended mission item.
Definition: move_action.h:56
Coordinates tuple for geodetic CS.
Definition: coordinates.h:41
Type
Types of vehicle actions as part of task (mission).
Definition: action.h:30
double heading
Heading in radians.
Definition: move_action.h:135
Generic action.
Definition: action.h:22
Move_action(const Property_list &p)
Construct move action from protobuf command.
Definition: move_action.h:79
#define ASSERT(x)
No action in release.
Definition: debug.h:68
Generic action.
Map Action type enum value to specific Action type class.
Definition: action.h:117
Turn_type
Turn type at waypoint.
Definition: move_action.h:31
Move_action(Wgs84_position position, double wait_time, double acceptance_radius, double loiter_orbit, double heading, double elevation)
Construct move action explicitly.
Definition: move_action.h:39
Move vehicle to the specific position (waypoint).
Definition: move_action.h:23
double loiter_orbit
Radius of the point fly-by orbit in meters.
Definition: move_action.h:130
double acceptance_radius
Acceptance radius of the target position in meters.
Definition: move_action.h:123
double wait_time
Wait interval in seconds.
Definition: move_action.h:116
#define M_PI
PI constant.
Definition: coordinates.h:21
Wgs84_position position
Target global position of the movement.
Definition: move_action.h:110
#define DEFINE_COMMON_CLASS(__class_name,...)
Use this macro to define some common attributes for a class.
Definition: utils.h:25
Coordinates manipulation.