VSM C++ SDK
Vehicle Specific Modules SDK
takeoff_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 
8 #ifndef _TAKEOFF_ACTION_H_
9 #define _TAKEOFF_ACTION_H_
10 
11 #include <ugcs/vsm/action.h>
12 #include <ugcs/vsm/optional.h>
13 
14 namespace ugcs {
15 namespace vsm {
16 
18 class Takeoff_action: public Action {
20 
21 public:
22 
25  double climb_rate, double acceptance_radius) :
26  Action(Type::TAKEOFF),
27  position(position),
28  heading(heading),
29  elevation(elevation),
30  climb_rate(climb_rate),
32 
38  Takeoff_action(const mavlink::ugcs::Pld_mission_item_ex& item, Optional<double>& takeoff_altitude) :
39  Action(Type::TAKEOFF),
40  position(Geodetic_tuple(item->x * M_PI / 180.0,
41  item->y * M_PI / 180.0,
42  item->z)),
43  heading(item->param4 * M_PI / 180.0),
44  elevation(item->elevation),
45  climb_rate(item->param3),
46  acceptance_radius(item->param2)
47  {
48  ASSERT(item->command == mavlink::ugcs::MAV_CMD::MAV_CMD_NAV_TAKEOFF_EX);
49  if (item->altitude_origin.Is_reset()) {
50  takeoff_altitude = item->elevation;
51  } else {
52  takeoff_altitude = item->altitude_origin;
53  }
54  }
55 
60  Action(Type::TAKEOFF),
61  position(Geodetic_tuple(0,0,0))
62  {
63  double lat = 0, lon = 0, alt = 0;
64  auto pi = p.find("latitude");
65  if (pi != p.end()) {
66  pi->second->Get_value(lat);
67  }
68  pi = p.find("longitude");
69  if (pi != p.end()) {
70  pi->second->Get_value(lon);
71  }
72  pi = p.find("altitude_amsl");
73  if (pi != p.end()) {
74  pi->second->Get_value(alt);
75  }
76  position = Geodetic_tuple(lat, lon, alt);
77  p.at("acceptance_radius")->Get_value(acceptance_radius);
78  p.at("heading")->Get_value(heading);
79  p.at("climb_rate")->Get_value(climb_rate);
80  p.at("ground_elevation")->Get_value(elevation);
81  }
82 
86 
88  double heading;
89 
91  double elevation;
92 
94  double climb_rate;
95 
99 };
100 
102 template<>
106 };
107 
108 } /* namespace vsm */
109 } /* namespace ugcs */
110 
111 #endif /* _TAKEOFF_ACTION_H_ */
UGCS root namespace.
Definition: android-linux/ugcs/vsm/platform_sockets.h:27
Definition: property.h:191
Takeoff_action type
Real type.
Definition: takeoff_action.h:105
Coordinates tuple for geodetic CS.
Definition: coordinates.h:41
Wgs84_position position
Take-off position.
Definition: takeoff_action.h:85
Type
Types of vehicle actions as part of task (mission).
Definition: action.h:30
double elevation
Terrain height in meters underneath the position.
Definition: takeoff_action.h:91
Generic action.
Definition: action.h:22
Takeoff_action(const Property_list &p)
Construct move action from protobuf command.
Definition: takeoff_action.h:59
double acceptance_radius
Acceptance radius of the position.
Definition: takeoff_action.h:98
#define ASSERT(x)
No action in release.
Definition: debug.h:68
Takeoff_action(const mavlink::ugcs::Pld_mission_item_ex &item, Optional< double > &takeoff_altitude)
Construct take-off action from Mavlink extended mission item.
Definition: takeoff_action.h:38
double climb_rate
Climbing rate, in meters per second.
Definition: takeoff_action.h:94
Generic action.
Map Action type enum value to specific Action type class.
Definition: action.h:117
Takeoff action ugcs::vsm::Takeoff_action.
double heading
Heading in radians.
Definition: takeoff_action.h:88
#define M_PI
PI constant.
Definition: coordinates.h:21
Take off from the specified position and reach specified altitude.
Definition: takeoff_action.h:18
#define DEFINE_COMMON_CLASS(__class_name,...)
Use this macro to define some common attributes for a class.
Definition: utils.h:25