forked from mavlink/MAVSDK
-
Notifications
You must be signed in to change notification settings - Fork 0
/
offboard.h
159 lines (137 loc) · 4.8 KB
/
offboard.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
#pragma once
#include <functional>
namespace dronecore {
class OffboardImpl;
/**
* @brief This class is used to control a drone with velocity commands.
*
* The module is called offboard because the velocity commands can be sent from external sources
* as opposed to onboard control right inside the autopilot "board".
*
* Client code must specify a setpoint before starting offboard mode.
* DroneCore automatically resends setpoints at 20Hz (PX4 Offboard mode requires that setpoints are
* minimally resent at 2Hz). If more precise control is required, clients can call the
* setpoint methods at whatever rate is required.
*
* **Attention:** this is work in progress, use with caution!
*/
class Offboard
{
public:
/**
* @brief Constructor (internal use only).
*/
explicit Offboard(OffboardImpl *impl);
/**
* @brief Destructor (internal use only).
*/
~Offboard();
/**
* @brief Results for offboard requests.
*/
enum class Result {
SUCCESS = 0, /**< @brief %Request succeeded. */
NO_DEVICE, /**< @brief No device connected. */
CONNECTION_ERROR, /**< @brief %Connection error. */
BUSY, /**< @brief Vehicle busy. */
COMMAND_DENIED, /**< @brief Command denied. */
TIMEOUT, /**< @brief %Request timeout. */
NO_SETPOINT_SET, /**< Can't start without setpoint set. */
UNKNOWN /**< @brief Unknown error. */
};
/**
* @brief Get human-readable English string for Offboard::Result.
*
* @param result The enum value for which a string is required.
* @returns Human-readable string for enum value.
*/
static const char *result_str(Result result);
/**
* @brief Callback type for offboard requests.
*/
typedef std::function<void(Result)> result_callback_t;
/**
* @brief Type for Velocity commands in NED (North East Down) coordinates and yaw.
*/
struct VelocityNEDYaw {
float north_m_s; /**< @brief Velocity North in metres/second. */
float east_m_s; /**< @brief Velocity East in metres/second. */
float down_m_s; /**< @brief Velocity Down in metres/second. */
float yaw_deg; /**< @brief Yaw in degrees (0 North, positive is clock-wise looking from above). */
};
/**
* @brief Type for velocity commands in body coordinates (forward, right, down and yaw angular rate).
*/
struct VelocityBodyYawspeed {
float forward_m_s; /**< @brief Velocity forward in metres/second. */
float right_m_s; /**< @brief Velocity right in metres/secon.d */
float down_m_s; /**< @brief Velocity down in metres/second. */
float yawspeed_deg_s; /**< @brief Yaw angular rate in degrees/second (positive for clock-wise
looking from above). */
};
/**
* @brief Start offboard control (synchronous).
*
* **Attention:** this is work in progress, use with caution!
*
* @return Result of request.
*/
Offboard::Result start();
/**
* @brief Stop offboard control (synchronous).
*
* The vehicle will be put into Hold mode: https://docs.px4.io/en/flight_modes/hold.html
*
* @return Result of request.
*/
Offboard::Result stop();
/**
* @brief Start offboard control (asynchronous).
*
* **Attention:** This is work in progress, use with caution!
*
* @param callback Callback to receive request result.
*/
void start_async(result_callback_t callback);
/**
* @brief Stop offboard control (asynchronous).
*
* The vehicle will be put into Hold mode: https://docs.px4.io/en/flight_modes/hold.html
*
* @param callback Callback to receive request result.
*/
void stop_async(result_callback_t callback);
/**
* @brief Check if offboard control is active.
*
* `true` means that the vehicle is in offboard mode and we are actively sending
* setpoints.
*
* @return `true` if active
*/
bool is_active() const;
/**
* @brief Set the velocity in NED coordinates and yaw.
*
* @param velocity_ned_yaw Velocity and yaw `struct`.
*/
void set_velocity_ned(VelocityNEDYaw velocity_ned_yaw);
/**
* @brief Set the velocity body coordinates coordinates and yaw angular rate.
*
* @param velocity_body_yawspeed Velocity and yaw angular rate `struct`.
*/
void set_velocity_body(VelocityBodyYawspeed velocity_body_yawspeed);
/**
* @brief Copy constructor (object is not copyable).
*/
Offboard(const Offboard &) = delete;
/**
* @brief Equality operator (object is not copyable).
*/
const Offboard &operator=(const Offboard &) = delete;
private:
/** @private Underlying implementation, set at instantiation */
OffboardImpl *_impl;
};
} // namespace dronecore