INSTINCT Code Coverage Report

Directory: src/
File: util/Vendor/MavLink/autopilot_interface.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 18 18 100.0%
Functions: 4 4 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 /****************************************************************************
2 *
3 * Copyright (c) 2014 MAVlink Development Team. All rights reserved.
4 * Author: Trent Lukaczyk, <aerialhedgehog@gmail.com>
5 * Jaycee Lock, <jaycee.lock@gmail.com>
6 * Lorenz Meier, <lm@inf.ethz.ch>
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * 1. Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in
16 * the documentation and/or other materials provided with the
17 * distribution.
18 * 3. Neither the name PX4 nor the names of its contributors may be
19 * used to endorse or promote products derived from this software
20 * without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
29 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
30 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 ****************************************************************************/
36
37 /**
38 * @file autopilot_interface.hpp
39 *
40 * @brief Autopilot interface definition
41 *
42 * Functions for sending and recieving commands to an autopilot via MAVlink
43 *
44 * @author Trent Lukaczyk, <aerialhedgehog@gmail.com>
45 * @author Jaycee Lock, <jaycee.lock@gmail.com>
46 * @author Lorenz Meier, <lm@inf.ethz.ch>
47 *
48 */
49
50 #pragma once
51
52 #if __linux__ || __APPLE__
53 // ------------------------------------------------------------------------------
54 // Includes
55 // ------------------------------------------------------------------------------
56
57 #include "generic_port.hpp"
58
59 #include <signal.h> // NOLINT(hicpp-deprecated-headers,modernize-deprecated-headers) // FIXME: inclusion of deprecated C++ header 'signal.h'; consider using 'csignal' instead
60 #include <time.h> // NOLINT(hicpp-deprecated-headers,modernize-deprecated-headers) // FIXME: inclusion of deprecated C++ header 'time.h'; consider using 'ctime' instead
61 #include <sys/time.h>
62 #include <pthread.h> // This uses POSIX Threads
63 #include <unistd.h> // UNIX standard function definitions
64 #include <mutex>
65 #include <cstdint>
66 #include <memory>
67
68 #include <mavlink/common/mavlink.h>
69
70 // ------------------------------------------------------------------------------
71 // Defines
72 // ------------------------------------------------------------------------------
73
74 /**
75 * Defines for mavlink_set_position_target_local_ned_t.type_mask
76 *
77 * Bitmask to indicate which dimensions should be ignored by the vehicle
78 *
79 * a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of
80 * the setpoint dimensions should be ignored.
81 *
82 * If bit 10 is set the floats afx afy afz should be interpreted as force
83 * instead of acceleration.
84 *
85 * Mapping:
86 * bit 1: x,
87 * bit 2: y,
88 * bit 3: z,
89 * bit 4: vx,
90 * bit 5: vy,
91 * bit 6: vz,
92 * bit 7: ax,
93 * bit 8: ay,
94 * bit 9: az,
95 * bit 10: is force setpoint,
96 * bit 11: yaw,
97 * bit 12: yaw rate
98 * remaining bits unused
99 *
100 * Combine bitmasks with bitwise &
101 *
102 * Example for position and yaw angle:
103 * uint16_t type_mask =
104 * MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION &
105 * MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE;
106 */
107
108 // bit number 876543210987654321
109 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION = 0b0000110111111000; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION
110 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY = 0b0000110111000111; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY
111 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION = 0b0000110000111111; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION
112 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_FORCE = 0b0000111000111111; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_FORCE
113 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE = 0b0000100111111111; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE
114 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE = 0b0000010111111111; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE
115
116 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_TAKEOFF = 0x1000; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_TAKEOFF
117 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LAND = 0x2000; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LAND
118 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LOITER = 0x3000; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_LOITER
119 constexpr size_t MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_IDLE = 0x4000; ///< MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_IDLE
120
121 /// Autopilot modes
122 enum AUTOPILOT_MODE_ : uint8_t
123 {
124 AUTOPILOT_MODE_STABILIZED = 0, ///< Self-levels the roll and pitch axis
125 AUTOPILOT_MODE_ACRO = 1, ///< Holds attitude, no self-level
126 AUTOPILOT_MODE_ALTHOLD = 2, ///< Holds altitude and self-levels the roll & pitch
127 AUTOPILOT_MODE_AUTO = 3, ///< Executes pre-defined mission
128 AUTOPILOT_MODE_GUIDED = 4, ///< Navigates to single points commanded by GCS
129 AUTOPILOT_MODE_LOITER = 5, ///< Holds altitude and position, uses GPS for movements
130 AUTOPILOT_MODE_RTL = 6, ///< Returns above takeoff location, may also include landing
131 AUTOPILOT_MODE_CIRCLE = 7, ///< Automatically circles a point in front of the vehicle
132 AUTOPILOT_MODE_LAND = 9, ///< Reduces altitude to ground level, attempts to go straight down
133 AUTOPILOT_MODE_DRIFT = 11, ///< Like stabilize, but coordinates yaw with roll like a plane
134 AUTOPILOT_MODE_SPORT = 13, ///< Alt-hold, but holds pitch & roll when sticks centered
135 AUTOPILOT_MODE_FLIP = 14, ///< Rises and completes an automated flip
136 AUTOPILOT_MODE_AUTOTUNE = 15, ///< Automated pitch and bank procedure to improve control loops
137 AUTOPILOT_MODE_POSHOLD = 16, ///< Like loiter, but manual roll and pitch when sticks not centered
138 AUTOPILOT_MODE_BRAKE = 17, ///< Brings copter to an immediate stop
139 AUTOPILOT_MODE_THROW = 18, ///< Holds position after a throwing takeoff
140 AUTOPILOT_MODE_AVOIDADSB = 19, ///< ADS-B based avoidance of manned aircraft
141 AUTOPILOT_MODE_GUIDEDNOGPS = 20, ///< This variation of Guided mode does not require a GPS but it only accepts attitude targets
142 AUTOPILOT_MODE_SMARTRTL = 21, ///< RTL, but traces path to get home
143 AUTOPILOT_MODE_FOLOWHOLD = 22, ///< Position control using Optical Flow
144 AUTOPILOT_MODE_FOLLOW = 23, ///< Follows another vehicle
145 AUTOPILOT_MODE_ZIGZAG = 24, ///< Useful for crop spraying
146 AUTOPILOT_MODE_SYSTEMID = 25, ///< Special diagnostic/modeling mode
147 };
148
149 // ------------------------------------------------------------------------------
150 // Prototypes
151 // ------------------------------------------------------------------------------
152
153 // helper functions
154
155 /// @brief Get time in [µs]
156 uint64_t get_time_usec();
157
158 /// @brief Set position in NED coordinates
159 /// @param[in] x desired position value, north component
160 /// @param[in] y desired position value, east component
161 /// @param[in] z desired position value, down component
162 /// @param[in] sp Mavlink set position target local NED
163 void set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t& sp);
164
165 /// @brief Set velocity in NED coordinates
166 /// @param[in] vx desired velocity value, north component
167 /// @param[in] vy desired velocity value, east component
168 /// @param[in] vz desired velocity value, down component
169 /// @param[in] sp Mavlink set position target local NED
170 void set_velocity(float vx, float vy, float vz, mavlink_set_position_target_local_ned_t& sp);
171
172 /// @brief Set acceleration in NED coordinates
173 /// @param[in] ax desired acceleration value, north component
174 /// @param[in] ay desired acceleration value, east component
175 /// @param[in] az desired acceleration value, down component
176 /// @param[in] sp Mavlink set position target local NED
177 void set_acceleration(float ax, float ay, float az, mavlink_set_position_target_local_ned_t& sp);
178
179 /// @brief Set yaw angle
180 /// @param[in] yaw desired yaw angle
181 /// @param[in] sp Mavlink set position target local NED
182 void set_yaw(float yaw, mavlink_set_position_target_local_ned_t& sp);
183
184 /// @brief Set rate of yaw angle
185 /// @param[in] yaw_rate desired rate of yaw angle
186 /// @param[in] sp Mavlink set position target local NED
187 void set_yaw_rate(float yaw_rate, mavlink_set_position_target_local_ned_t& sp);
188
189 /// @brief Start autopilot interface read thread
190 /// @param[in] args Arguments
191 void* start_autopilot_interface_read_thread(void* args);
192
193 /// @brief Start autopilot interface write thread
194 /// @param[in] args Arguments
195 void* start_autopilot_interface_write_thread(void* args);
196
197 // ------------------------------------------------------------------------------
198 // Data Structures
199 // ------------------------------------------------------------------------------
200
201 /// @brief Time Stamps
202 struct Time_Stamps
203 {
204 112 Time_Stamps()
205 112 {
206 112 reset_timestamps();
207 112 }
208
209 uint64_t heartbeat{}; ///< heartbeat
210 uint64_t sys_status{}; ///< sys_status
211 uint64_t battery_status{}; ///< battery_status
212 uint64_t radio_status{}; ///< radio_status
213 uint64_t local_position_ned{}; ///< local_position_ned
214 uint64_t global_position_int{}; ///< global_position_int
215 uint64_t position_target_local_ned{}; ///< position_target_local_ned
216 uint64_t position_target_global_int{}; ///< position_target_global_int
217 uint64_t highres_imu{}; ///< highres_imu
218 uint64_t attitude{}; ///< attitude
219
220 /// @brief Reset the timestamps to zero
221 112 void reset_timestamps()
222 {
223 112 heartbeat = 0;
224 112 sys_status = 0;
225 112 battery_status = 0;
226 112 radio_status = 0;
227 112 local_position_ned = 0;
228 112 global_position_int = 0;
229 112 position_target_local_ned = 0;
230 112 position_target_global_int = 0;
231 112 highres_imu = 0;
232 112 attitude = 0;
233 112 }
234 };
235
236 /// @brief Struct containing information on the MAV we are currently connected to
237 struct Mavlink_Messages
238 {
239 int sysid{}; ///< System ID
240 int compid{}; ///< Component ID
241
242 mavlink_heartbeat_t heartbeat{}; ///< Heartbeat
243 mavlink_sys_status_t sys_status{}; ///< System Status
244 mavlink_battery_status_t battery_status{}; ///< Battery Status
245 mavlink_radio_status_t radio_status{}; ///< Radio Status
246 mavlink_local_position_ned_t local_position_ned{}; ///< Local Position
247 mavlink_global_position_int_t global_position_int{}; ///< Global Position
248 mavlink_position_target_local_ned_t position_target_local_ned{}; ///< Local Position Target
249 mavlink_position_target_global_int_t position_target_global_int{}; ///< Global Position Target
250 mavlink_highres_imu_t highres_imu{}; ///< HiRes IMU
251 mavlink_attitude_t attitude{}; ///< Attitude
252
253 // System Parameters?
254
255 Time_Stamps time_stamps; ///< Time Stamps
256
257 /// @brief Reset timestamps
258 void reset_timestamps()
259 {
260 time_stamps.reset_timestamps();
261 }
262 };
263
264 // ----------------------------------------------------------------------------------
265 // Autopilot Interface Class
266 // ----------------------------------------------------------------------------------
267 /*
268 * Autopilot Interface Class
269 *
270 * This starts two threads for read and write over MAVlink. The read thread
271 * listens for any MAVlink message and pushes it to the current_messages
272 * attribute. The write thread at the moment only streams a position target
273 * in the local NED frame (mavlink_set_position_target_local_ned_t), which
274 * is changed by using the method update_setpoint(). Sending these messages
275 * are only half the requirement to get response from the autopilot, a signal
276 * to enter "offboard_control" mode is sent by using the enable_offboard_control()
277 * method. Signal the exit of this mode with disable_offboard_control(). It's
278 * important that one way or another this program signals offboard mode exit,
279 * otherwise the vehicle will go into failsafe.
280 */
281
282 /// @brief Autopilot Interface Class
283 class Autopilot_Interface
284 {
285 public:
286 /// @brief Default constructor
287 112 Autopilot_Interface() = default;
288 /// @brief Constructor
289 explicit Autopilot_Interface(const std::shared_ptr<Generic_Port>& port_);
290 /// @brief Destructor
291 112 ~Autopilot_Interface() = default;
292 /// @brief Copy constructor
293 Autopilot_Interface(const Autopilot_Interface&) = delete;
294 /// @brief Move constructor
295 Autopilot_Interface(Autopilot_Interface&&) = delete;
296 /// @brief Copy assignment operator
297 Autopilot_Interface& operator=(const Autopilot_Interface&) = delete;
298 /// @brief Move assignment operator
299 Autopilot_Interface& operator=(Autopilot_Interface&&) = delete;
300
301 uint64_t write_count{}; ///< Write count
302 char reading_status{}; ///< Reading status
303 char writing_status{}; ///< Writing status
304 char control_status{}; ///< Control status
305
306 int system_id{}; ///< System ID
307 int autopilot_id{}; ///< Autopilot ID
308 int companion_id{}; ///< Companion ID
309
310 // MOCAP Variables
311 float mocap_w{}; ///< MOCAP quaternion, scalar part
312 float mocap_x{}; ///< MOCAP quaternion, vector component x
313 float mocap_y{}; ///< MOCAP quaternion, vector component y
314 float mocap_z{}; ///< MOCAP quaternion, vector component z
315 bool MOCAP_Active = false; ///< Flag to indicate whether MOCAP is active
316 double MOCAP_Frequency = 0; ///< MOCAP frequency
317
318 // GPS Variables
319 int32_t gps_lat_d{}; ///< GPS latitude
320 int32_t gps_lon_d{}; ///< GPS longitude
321 float gps_alt{}; ///< GPS altitude
322 float gps_vn{}; ///< GPS velocity north component
323 float gps_ve{}; ///< GPS velocity east component
324 float gps_vd{}; ///< GPS velocity down component
325 uint32_t gps_time_week_ms{}; ///< GPS time of week in [ms]
326 uint16_t gps_time_week{}; ///< GPS time of week
327 bool GPS_Active = false; ///< GPS active
328 double GPS_Frequency = 0; ///< GPS frequency
329
330 Mavlink_Messages current_messages{}; ///< Current Mavlink message
331 mavlink_set_position_target_local_ned_t initial_position{}; ///< Initial position for Mavlink set position target local NED
332
333 /// @brief Update setpoint of Mavlink position target in NED
334 /// @param[in] setpoint Setpoint of Mavlink position target in NED
335 void update_setpoint(mavlink_set_position_target_local_ned_t setpoint);
336
337 /// @brief Read messages
338 void read_messages();
339
340 /// @brief Write messages
341 /// @param[in] message Mavlink message to write
342 int write_message(mavlink_message_t message);
343
344 /// @brief Arm / disarm
345 /// @param[in] flag flag
346 int arm_disarm(bool flag);
347
348 /// @brief Enable offboard control
349 void enable_offboard_control();
350
351 /// @brief Disable offboard control
352 void disable_offboard_control();
353
354 /// @brief Start
355 void start();
356
357 /// @brief Stop
358 void stop();
359
360 /// @brief Start read thread
361 void start_read_thread();
362
363 /// @brief Start write thread
364 void start_write_thread();
365
366 /// @brief Handle quit
367 /// @param[in] sig Signal
368 void handle_quit(int sig);
369
370 /// @brief GPS input
371 /// @param[in] lat_d Latitude
372 /// @param[in] lon_d Longitude
373 /// @param[in] alt Altitude
374 /// @param[in] vn Velocity north
375 /// @param[in] ve Velocity east
376 /// @param[in] vd Velocity down
377 /// @param[in] time_week_ms Time of week in [ms]
378 /// @param[in] time_week Time of week
379 int GPS_Input(int32_t lat_d, int32_t lon_d, float alt, float vn, float ve, float vd, uint32_t time_week_ms, uint16_t time_week);
380
381 /// @brief MOCAP input
382 /// @param[in] w scalar part
383 /// @param[in] x vector component x
384 /// @param[in] y vector component y
385 /// @param[in] z vector component z
386 int MOCAP_Input(float w, float x, float y, float z);
387
388 /// @brief Set MOCAP
389 /// @param[in] w scalar part
390 /// @param[in] x vector component x
391 /// @param[in] y vector component y
392 /// @param[in] z vector component z
393 void setMOCAP(float w, float x, float y, float z);
394
395 /// @brief Set GPS
396 /// @param[in] lat_d Latitude
397 /// @param[in] lon_d Longitude
398 /// @param[in] alt Altitude
399 /// @param[in] vn Velocity north
400 /// @param[in] ve Velocity east
401 /// @param[in] vd Velocity down
402 /// @param[in] time_week_ms Time of week in [ms]
403 /// @param[in] time_week Time of week
404 void setGPS(int32_t lat_d, int32_t lon_d, float alt, float vn, float ve, float vd, uint32_t time_week_ms, uint16_t time_week);
405
406 /// @brief Set port
407 /// @param[in] port_new Generic port
408 void setPort(const std::shared_ptr<Generic_Port>& port_new);
409
410 /// @brief Set GPS frequency
411 /// @param[in] _Frequency Frequency to set
412 void setGPS_Frequency(double _Frequency);
413
414 /// @brief Set MOCAP frequency
415 /// @param[in] _Frequency Frequency to set
416 void setMOCAP_Frequency(double _Frequency);
417
418 /// @brief Set GPS active
419 /// @param[in] _Active Flag that indicates whether GPS is active
420 void setGPS_Active(bool _Active);
421
422 /// @brief Set MOCAP active
423 /// @param[in] _Active Flag that indicates whether MOCAP is active
424 void setMOCAP_Active(bool _Active);
425
426 private:
427 std::shared_ptr<Generic_Port> port; ///< Generic port
428
429 bool time_to_exit = false; ///< Time to exit
430
431 pthread_t read_tid{}; ///< pthread read
432 pthread_t write_tid{}; ///< pthread write
433
434 /// @brief Current setpoint
435 struct
436 {
437 std::mutex mutex; ///< mutex
438 mavlink_set_position_target_local_ned_t data{}; ///< data
439 } current_setpoint;
440
441 /// @brief Read Thread
442 void read_thread();
443
444 /// @brief Write Thread
445 void write_thread();
446
447 /// @brief Toggle offboard control
448 /// @param[in] flag Flag for the toggle
449 int toggle_offboard_control(bool flag);
450
451 /// @brief Write setpoint
452 void write_setpoint();
453 };
454
455 #endif
456