INSTINCT Code Coverage Report

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.cpp
39			*
40			* @brief Autopilot interface functions
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			// ------------------------------------------------------------------------------
51			// Includes
52			// ------------------------------------------------------------------------------
53
54			#if __linux__ || __APPLE__
55			#include <optional>
56
57			#include "autopilot_interface.hpp"
58			#include "util/Logger.hpp"
59
60			// ----------------------------------------------------------------------------------
61			// Time
62			// ----------------------------------------------------------------------------------
63		✗	uint64_t get_time_usec()
64			{
65			static struct timeval _time_stamp;
66		✗	gettimeofday(&_time_stamp, nullptr);
67		✗	return static_cast<uint64_t>(_time_stamp.tv_sec) * 1000000UL + static_cast<uint64_t>(_time_stamp.tv_usec);
68			}
69
70			// ----------------------------------------------------------------------------------
71			// Setpoint Helper Functions
72			// ----------------------------------------------------------------------------------
73
74			// choose one of the next three
75
76			/*
77			* Set target local ned position
78			*
79			* Modifies a mavlink_set_position_target_local_ned_t struct with target XYZ locations
80			* in the Local NED frame, in meters.
81			*/
82		✗	void set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t& sp)
83			{
84		✗	sp.type_mask =
85			MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION;
86
87		✗	sp.coordinate_frame = MAV_FRAME_LOCAL_NED;
88
89		✗	sp.x = x;
90		✗	sp.y = y;
91		✗	sp.z = z;
92
93		✗	LOG_INFO("autopilot_interface.cpp - POSITION SETPOINT XYZ = [ {} , {} , {} ] \n", sp.x, sp.y, sp.z);
94		✗	}
95
96			/*
97			* Set target local ned velocity
98			*
99			* Modifies a mavlink_set_position_target_local_ned_t struct with target VX VY VZ
100			* velocities in the Local NED frame, in meters per second.
101			*/
102		✗	void set_velocity(float vx, float vy, float vz, mavlink_set_position_target_local_ned_t& sp)
103			{
104		✗	sp.type_mask =
105			MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY;
106
107		✗	sp.coordinate_frame = MAV_FRAME_LOCAL_NED;
108
109		✗	sp.vx = vx;
110		✗	sp.vy = vy;
111		✗	sp.vz = vz;
112
113		✗	LOG_INFO("autopilot_interface.cpp - VELOCITY SETPOINT UVW = [ {} , {} , {} ] \n", sp.vx, sp.vy, sp.vz);
114		✗	}
115
116			/*
117			* Set target local ned acceleration
118			*
119			* Modifies a mavlink_set_position_target_local_ned_t struct with target AX AY AZ
120			* accelerations in the Local NED frame, in meters per second squared.
121			*/
122		✗	void set_acceleration(float /* ax */, float /* ay */, float /* az */, mavlink_set_position_target_local_ned_t& /* sp */)
123			{
124			// NOT IMPLEMENTED
125		✗	LOG_ERROR("autopilot_interface.cpp - set_acceleration doesn't work yet \n");
126
127			// sp.type_mask =
128			// MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION & MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY;
129
130			// sp.coordinate_frame = MAV_FRAME_LOCAL_NED;
131
132			// sp.afx = ax;
133			// sp.afy = ay;
134			// sp.afz = az;
135		✗	}
136
137			// the next two need to be called after one of the above
138
139			/*
140			* Set target local ned yaw
141			*
142			* Modifies a mavlink_set_position_target_local_ned_t struct with a target yaw
143			* in the Local NED frame, in radians.
144			*/
145		✗	void set_yaw(float yaw, mavlink_set_position_target_local_ned_t& sp)
146			{
147		✗	sp.type_mask &=
148			MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE;
149
150		✗	sp.yaw = yaw;
151
152		✗	LOG_INFO("autopilot_interface.cpp - POSITION SETPOINT YAW = {} \n", sp.yaw);
153		✗	}
154
155			/*
156			* Set target local ned yaw rate
157			*
158			* Modifies a mavlink_set_position_target_local_ned_t struct with a target yaw rate
159			* in the Local NED frame, in radians per second.
160			*/
161		✗	void set_yaw_rate(float yaw_rate, mavlink_set_position_target_local_ned_t& sp)
162			{
163		✗	sp.type_mask &=
164			MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE;
165
166		✗	sp.yaw_rate = yaw_rate;
167		✗	}
168
169			// ----------------------------------------------------------------------------------
170			// Autopilot Interface Class
171			// ----------------------------------------------------------------------------------
172
173			// ------------------------------------------------------------------------------
174			// Con/De structors
175			// ------------------------------------------------------------------------------
176		✗	Autopilot_Interface::Autopilot_Interface(const std::shared_ptr<Generic_Port>& port_) : port(port_)
177			{
178		✗	current_messages.sysid = system_id;
179		✗	current_messages.compid = autopilot_id;
180		✗	}
181
182			// ------------------------------------------------------------------------------
183			// Set Port
184			// ------------------------------------------------------------------------------
185		✗	void Autopilot_Interface::setPort(const std::shared_ptr<Generic_Port>& port_new)
186			{
187		✗	port = port_new;
188		✗	}
189
190			// ------------------------------------------------------------------------------
191			// Update Setpoint
192			// ------------------------------------------------------------------------------
193		✗	void Autopilot_Interface::update_setpoint(mavlink_set_position_target_local_ned_t setpoint)
194			{
195		✗	std::lock_guard<std::mutex> lock(current_setpoint.mutex);
196		✗	current_setpoint.data = setpoint;
197		✗	}
198
199			// ------------------------------------------------------------------------------
200			// Read Messages
201			// ------------------------------------------------------------------------------
202		✗	void Autopilot_Interface::read_messages()
203			{
204		✗	bool success = false; // receive success flag
205		✗	bool received_all = false; // receive only one message
206		✗	Time_Stamps this_timestamps;
207
208			// Blocking wait for new data
209		✗	while (!received_all and !time_to_exit)
210			{
211			// ----------------------------------------------------------------------
212			// READ MESSAGE
213			// ----------------------------------------------------------------------
214			mavlink_message_t message;
215		✗	success = port->read_message(message);
216
217			// ----------------------------------------------------------------------
218			// HANDLE MESSAGE
219			// ----------------------------------------------------------------------
220		✗	if (success)
221			{
222			// Store message sysid and compid.
223			// Note this doesn't handle multiple message sources.
224		✗	current_messages.sysid = message.sysid;
225		✗	current_messages.compid = message.compid;
226
227			// Handle Message ID
228		✗	switch (message.msgid)
229			{
230		✗	case MAVLINK_MSG_ID_HEARTBEAT:
231			{
232			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_HEARTBEAT\n");
233		✗	mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
234		✗	current_messages.time_stamps.heartbeat = get_time_usec();
235		✗	this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
236		✗	break;
237			}
238
239		✗	case MAVLINK_MSG_ID_SYS_STATUS:
240			{
241			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_SYS_STATUS\n");
242		✗	mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
243		✗	current_messages.time_stamps.sys_status = get_time_usec();
244		✗	this_timestamps.sys_status = current_messages.time_stamps.sys_status;
245		✗	break;
246			}
247
248		✗	case MAVLINK_MSG_ID_BATTERY_STATUS:
249			{
250			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_BATTERY_STATUS\n");
251		✗	mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
252		✗	current_messages.time_stamps.battery_status = get_time_usec();
253		✗	this_timestamps.battery_status = current_messages.time_stamps.battery_status;
254		✗	break;
255			}
256
257		✗	case MAVLINK_MSG_ID_RADIO_STATUS:
258			{
259			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_RADIO_STATUS\n");
260		✗	mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
261		✗	current_messages.time_stamps.radio_status = get_time_usec();
262		✗	this_timestamps.radio_status = current_messages.time_stamps.radio_status;
263		✗	break;
264			}
265
266		✗	case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
267			{
268			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
269		✗	mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
270		✗	current_messages.time_stamps.local_position_ned = get_time_usec();
271		✗	this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
272		✗	break;
273			}
274
275		✗	case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
276			{
277			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
278		✗	mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
279		✗	current_messages.time_stamps.global_position_int = get_time_usec();
280		✗	this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
281		✗	break;
282			}
283
284		✗	case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
285			{
286			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
287		✗	mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
288		✗	current_messages.time_stamps.position_target_local_ned = get_time_usec();
289		✗	this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
290		✗	break;
291			}
292
293		✗	case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
294			{
295			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
296		✗	mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
297		✗	current_messages.time_stamps.position_target_global_int = get_time_usec();
298		✗	this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
299		✗	break;
300			}
301
302		✗	case MAVLINK_MSG_ID_HIGHRES_IMU:
303			{
304			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_HIGHRES_IMU\n");
305		✗	mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
306		✗	current_messages.time_stamps.highres_imu = get_time_usec();
307		✗	this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
308		✗	break;
309			}
310
311		✗	case MAVLINK_MSG_ID_ATTITUDE:
312			{
313			// LOG_INFO("autopilot_interface.cpp - MAVLINK_MSG_ID_ATTITUDE\n");
314		✗	mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
315		✗	current_messages.time_stamps.attitude = get_time_usec();
316		✗	this_timestamps.attitude = current_messages.time_stamps.attitude;
317		✗	break;
318			}
319
320		✗	default:
321			{
322			// LOG_WARN("Warning, did not handle message id %i\n",message.msgid);
323		✗	break;
324			}
325
326			} // end: switch msgid
327
328			} // end: if read message
329
330			// Check for receipt of all items
331		✗	received_all =
332		✗	this_timestamps.heartbeat &&
333			// this_timestamps.battery_status &&
334			// this_timestamps.radio_status &&
335			// this_timestamps.local_position_ned &&
336			// this_timestamps.global_position_int &&
337			// this_timestamps.position_target_local_ned &&
338			// this_timestamps.position_target_global_int &&
339			// this_timestamps.highres_imu &&
340			// this_timestamps.attitude &&
341		✗	this_timestamps.sys_status;
342
343			// give the write thread time to use the port
344		✗	if (writing_status > false)
345			{
346		✗	usleep(100); // look for components of batches at 10kHz
347			}
348
349			} // end: while not received all
350		✗	}
351
352			// ------------------------------------------------------------------------------
353			// Write Message
354			// ------------------------------------------------------------------------------
355		✗	int Autopilot_Interface::write_message(mavlink_message_t message)
356			{
357			// do the write
358		✗	int len = port->write_message(message);
359
360			// book keep
361		✗	write_count++;
362
363			// Done!
364		✗	return len;
365			}
366
367			// ------------------------------------------------------------------------------
368			// Write Setpoint Message
369			// ------------------------------------------------------------------------------
370		✗	void Autopilot_Interface::write_setpoint()
371			{
372			// --------------------------------------------------------------------------
373			// PACK PAYLOAD
374			// --------------------------------------------------------------------------
375
376			// pull from position target
377			mavlink_set_position_target_local_ned_t sp;
378			{
379		✗	std::lock_guard<std::mutex> lock(current_setpoint.mutex);
380		✗	sp = current_setpoint.data;
381		✗	}
382
383			// double check some system parameters
384
385		✗	if (!sp.time_boot_ms)
386			{
387		✗	sp.time_boot_ms = static_cast<uint32_t>(get_time_usec() / 1000);
388			}
389		✗	sp.target_system = static_cast<uint8_t>(system_id);
390		✗	sp.target_component = static_cast<uint8_t>(autopilot_id);
391
392			// --------------------------------------------------------------------------
393			// ENCODE
394			// --------------------------------------------------------------------------
395
396			mavlink_message_t message;
397		✗	mavlink_msg_set_position_target_local_ned_encode(static_cast<uint8_t>(system_id), static_cast<uint8_t>(companion_id), &message, &sp);
398
399			// --------------------------------------------------------------------------
400			// WRITE
401			// --------------------------------------------------------------------------
402
403			// do the write
404		✗	int len = write_message(message);
405
406			// check the write
407		✗	if (len <= 0)
408			{
409		✗	LOG_WARN("autopilot_interface.cpp - could not send POSITION_TARGET_LOCAL_NED \n");
410			}
411			// else
412			// LOG_INFO("autopilot_interface.cpp - {} POSITION_TARGET = [ {} , {} , {} ] \n", write_count, position_target.x, position_target.y, position_target.z);
413		✗	}
414
415			// ------------------------------------------------------------------------------
416			// Start Off-Board Mode
417			// ------------------------------------------------------------------------------
418		✗	void Autopilot_Interface::enable_offboard_control()
419			{
420			// Should only send this command once
421		✗	if (!static_cast<bool>(control_status))
422			{
423		✗	LOG_INFO("autopilot_interface.cpp - ENABLE OFFBOARD MODE\n");
424
425			// ----------------------------------------------------------------------
426			// TOGGLE OFF-BOARD MODE
427			// ----------------------------------------------------------------------
428
429			// Sends the command to go off-board
430		✗	int success = toggle_offboard_control(true);
431
432			// Check the command was written
433		✗	if (success)
434			{
435		✗	control_status = true;
436			}
437			else
438			{
439		✗	LOG_ERROR("autopilot_interface.cpp - off-board mode not set, could not write message\n");
440		✗	return;
441			}
442
443			} // end: if not offboard_status
444			}
445
446			// ------------------------------------------------------------------------------
447			// Stop Off-Board Mode
448			// ------------------------------------------------------------------------------
449		✗	void Autopilot_Interface::disable_offboard_control()
450			{
451			// Should only send this command once
452		✗	if (static_cast<bool>(control_status))
453			{
454		✗	LOG_INFO("autopilot_interface.cpp - DISABLE OFFBOARD MODE\n");
455
456			// ----------------------------------------------------------------------
457			// TOGGLE OFF-BOARD MODE
458			// ----------------------------------------------------------------------
459
460			// Sends the command to stop off-board
461		✗	int success = toggle_offboard_control(false);
462
463			// Check the command was written
464		✗	if (success)
465			{
466		✗	control_status = false;
467			}
468			else
469			{
470		✗	LOG_ERROR("autopilot_interface.cpp - off-board mode not set, could not write message\n");
471		✗	return;
472			}
473
474			} // end: if offboard_status
475			}
476
477			// ------------------------------------------------------------------------------
478			// Arm
479			// ------------------------------------------------------------------------------
480		✗	int Autopilot_Interface::arm_disarm(bool flag)
481			{
482		✗	if (flag)
483			{
484		✗	LOG_INFO("autopilot_interface.cpp - ARM ROTORS\n");
485			}
486			else
487			{
488		✗	LOG_INFO("autopilot_interface.cpp - DISARM ROTORS\n");
489			}
490
491			// Prepare command for off-board mode
492		✗	mavlink_command_long_t com = { 0, NAN, NAN, NAN, NAN, NAN, NAN, 0, 0, 0, 0 }; // FIXME: init values could be wrong here, but have to be set
493		✗	com.target_system = static_cast<uint8_t>(system_id);
494		✗	com.target_component = static_cast<uint8_t>(autopilot_id);
495		✗	com.command = MAV_CMD_COMPONENT_ARM_DISARM;
496		✗	com.confirmation = true;
497		✗	com.param1 = static_cast<float>(flag);
498		✗	com.param2 = 21196.;
499
500			// Encode
501			mavlink_message_t message;
502		✗	mavlink_msg_command_long_encode(static_cast<uint8_t>(system_id), static_cast<uint8_t>(companion_id), &message, &com);
503
504			// Send the message
505		✗	int len = port->write_message(message);
506
507			// Done!
508		✗	return len;
509			}
510
511			// ------------------------------------------------------------------------------
512			// Toggle Off-Board Mode
513			// ------------------------------------------------------------------------------
514		✗	int Autopilot_Interface::toggle_offboard_control(bool flag)
515			{
516			// Prepare command for off-board mode
517		✗	mavlink_command_long_t com = { 0, NAN, NAN, NAN, NAN, NAN, NAN, 0, 0, 0, 0 }; // FIXME: init values could be wrong here, but have to be set
518		✗	com.target_system = static_cast<uint8_t>(system_id);
519		✗	com.target_component = static_cast<uint8_t>(autopilot_id);
520		✗	com.command = MAV_CMD_NAV_GUIDED_ENABLE;
521		✗	com.confirmation = true;
522		✗	com.param1 = static_cast<float>(flag); // flag >0.5 => start, <0.5 => stop
523
524			// Encode
525			mavlink_message_t message;
526		✗	mavlink_msg_command_long_encode(static_cast<uint8_t>(system_id), static_cast<uint8_t>(companion_id), &message, &com);
527
528			// Send the message
529		✗	int len = port->write_message(message);
530
531			// Done!
532		✗	return len;
533			}
534
535			// ------------------------------------------------------------------------------
536			// MOCAP_Input (ATT_POS_MOCAP (#138))
537			// ------------------------------------------------------------------------------
538		✗	int Autopilot_Interface::MOCAP_Input(float w, float x, float y, float z)
539			{
540		✗	mavlink_att_pos_mocap_t current_MOCAP{};
541
542		✗	current_MOCAP.time_usec = get_time_usec();
543		✗	current_MOCAP.q[0] = w; // Attitude quaternion W
544		✗	current_MOCAP.q[1] = x; // Attitude quaternion X
545		✗	current_MOCAP.q[2] = y; // Attitude quaternion Y
546		✗	current_MOCAP.q[3] = z; // Attitude quaternion Z
547		✗	current_MOCAP.x = 0; // X position (NED) (m)
548		✗	current_MOCAP.y = 0; // Y position (NED) (m)
549		✗	current_MOCAP.z = 0; // Z position (NED) (m)
550
551		✗	current_MOCAP.covariance[0] = { std::nanf("1") };
552			/*
553			current_MOCAP.covariance[1] = 0;
554			current_MOCAP.covariance[2] = 0;
555			current_MOCAP.covariance[3] = 0;
556			current_MOCAP.covariance[4] = 0;
557			current_MOCAP.covariance[5] = 0;
558
559			current_MOCAP.covariance[6] = 0;
560			current_MOCAP.covariance[7] = 0;
561			current_MOCAP.covariance[8] = 0;
562			current_MOCAP.covariance[9] = 0;
563			current_MOCAP.covariance[10] = 0;
564			current_MOCAP.covariance[11] = 0;
565			current_MOCAP.covariance[12] = 0;
566			current_MOCAP.covariance[13] = 0;
567			current_MOCAP.covariance[14] = 0;
568			current_MOCAP.covariance[15] = 0;
569			current_MOCAP.covariance[16] = 0;
570			current_MOCAP.covariance[17] = 0;
571			current_MOCAP.covariance[18] = 0;
572			current_MOCAP.covariance[19] = 0;
573			current_MOCAP.covariance[20] = 0;
574			*/
575
576			// Encode
577			mavlink_message_t message;
578		✗	mavlink_msg_att_pos_mocap_encode(static_cast<uint8_t>(system_id), static_cast<uint8_t>(companion_id), &message, &current_MOCAP); // NOLINT(clang-analyzer-core.CallAndMessage)
579
580			// Send the message
581		✗	int len = port->write_message(message);
582
583			// Done!
584		✗	return len;
585			}
586
587			// ------------------------------------------------------------------------------
588			// GPS_Input (GPS_INPUT (#232))
589			// ------------------------------------------------------------------------------
590
591		✗	int Autopilot_Interface::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)
592			{
593			mavlink_gps_input_t GPS;
594		✗	GPS.time_usec = get_time_usec();
595		✗	GPS.gps_id = 0;
596		✗	GPS.ignore_flags = GPS_INPUT_IGNORE_FLAG_HDOP | GPS_INPUT_IGNORE_FLAG_VDOP |
597			// GPS_INPUT_IGNORE_FLAG_VEL_HORIZ |
598			// GPS_INPUT_IGNORE_FLAG_VEL_VERT |
599			GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY | GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY | GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY;
600		✗	GPS.time_week_ms = time_week_ms;
601		✗	GPS.time_week = time_week;
602		✗	GPS.fix_type = 3;
603		✗	GPS.lat = lat_d; // Latitude (WGS84) (degE7)
604		✗	GPS.lon = lon_d; // Longitude (WGS84) (degE7)
605		✗	GPS.alt = alt; // Altitude (MSL) Positive for up (m)
606		✗	GPS.hdop = 0.0F;
607		✗	GPS.vdop = 0.0F;
608		✗	GPS.vn = vn;
609		✗	GPS.ve = ve;
610		✗	GPS.vd = vd;
611		✗	GPS.speed_accuracy = 0.0F;
612		✗	GPS.horiz_accuracy = 0.0F;
613		✗	GPS.vert_accuracy = 0.0F;
614		✗	GPS.satellites_visible = 20;
615		✗	GPS.yaw = 0;
616
617			// Encode
618			mavlink_message_t message;
619		✗	mavlink_msg_gps_input_encode(static_cast<uint8_t>(system_id), static_cast<uint8_t>(companion_id), &message, &GPS);
620
621			// Send the message
622		✗	int len = port->write_message(message);
623
624			// Done!
625		✗	return len;
626			}
627
628			// ------------------------------------------------------------------------------
629			// setGPS
630			// ------------------------------------------------------------------------------
631		✗	void Autopilot_Interface::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)
632			{
633		✗	gps_lat_d = lat_d;
634		✗	gps_lon_d = lon_d;
635		✗	gps_alt = alt;
636		✗	gps_vn = vn;
637		✗	gps_ve = ve;
638		✗	gps_vd = vd;
639		✗	gps_time_week_ms = time_week_ms;
640		✗	gps_time_week = time_week;
641		✗	}
642
643			// ------------------------------------------------------------------------------
644			// setMOCAP
645			// ------------------------------------------------------------------------------
646		✗	void Autopilot_Interface::setMOCAP(float w, float x, float y, float z)
647			{
648		✗	mocap_w = w;
649		✗	mocap_x = x;
650		✗	mocap_y = y;
651		✗	mocap_z = z;
652		✗	}
653
654			// ------------------------------------------------------------------------------
655			// setGPS_Active
656			// ------------------------------------------------------------------------------
657		✗	void Autopilot_Interface::setGPS_Active(bool _Active)
658			{
659		✗	GPS_Active = _Active;
660		✗	}
661
662			// ------------------------------------------------------------------------------
663			// setMOCAP_Active
664			// ------------------------------------------------------------------------------
665		✗	void Autopilot_Interface::setMOCAP_Active(bool _Active)
666			{
667		✗	MOCAP_Active = _Active;
668		✗	}
669
670			// ------------------------------------------------------------------------------
671			// setMOCAP_Frequency
672			// ------------------------------------------------------------------------------
673		✗	void Autopilot_Interface::setMOCAP_Frequency(double _Frequency)
674			{
675		✗	MOCAP_Frequency = _Frequency;
676		✗	}
677
678			// ------------------------------------------------------------------------------
679			// setGPS_Frequency
680			// ------------------------------------------------------------------------------
681		✗	void Autopilot_Interface::setGPS_Frequency(double _Frequency)
682			{
683		✗	GPS_Frequency = _Frequency;
684		✗	}
685
686			// ------------------------------------------------------------------------------
687			// STARTUP
688			// ------------------------------------------------------------------------------
689		✗	void Autopilot_Interface::start()
690			{
691		✗	std::optional<int> result;
692		✗	time_to_exit = false;
693		✗	current_messages = {};
694			// --------------------------------------------------------------------------
695			// CHECK PORT
696			// --------------------------------------------------------------------------
697
698		✗	if (!port->is_running()) // PORT_OPEN
699			{
700		✗	LOG_ERROR("autopilot_interface.cpp - port not open\n");
701		✗	time_to_exit = true;
702		✗	throw EXIT_FAILURE;
703			}
704
705			// --------------------------------------------------------------------------
706			// READ THREAD
707			// --------------------------------------------------------------------------
708
709		✗	LOG_INFO("autopilot_interface.cpp - START READ THREAD \n");
710
711		✗	result = pthread_create(&read_tid, nullptr, &start_autopilot_interface_read_thread, this);
712		✗	if (result.has_value())
713			{
714		✗	LOG_INFO("result = {}", result);
715			}
716
717			// now we're reading messages
718
719			// --------------------------------------------------------------------------
720			// CHECK FOR MESSAGES
721			// --------------------------------------------------------------------------
722
723		✗	LOG_INFO("autopilot_interface.cpp - CHECK FOR MESSAGES\n");
724		✗	int i = 0;
725		✗	while (!current_messages.sysid)
726			{
727		✗	if (time_to_exit || i >= 20)
728			{
729		✗	LOG_ERROR("autopilot_interface.cpp - NO MESSAGES FOUND\n");
730		✗	time_to_exit = true;
731		✗	throw EXIT_FAILURE;
732			}
733		✗	usleep(500000); // check at 2Hz
734		✗	i++; // wait only 10s
735			}
736
737		✗	LOG_INFO("autopilot_interface.cpp - Found\n");
738
739			// now we know autopilot is sending messages
740
741			// --------------------------------------------------------------------------
742			// GET SYSTEM and COMPONENT IDs
743			// --------------------------------------------------------------------------
744
745			// This comes from the heartbeat, which in theory should only come from
746			// the autopilot we're directly connected to it. If there is more than one
747			// vehicle then we can't expect to discover id's like this.
748			// In which case set the id's manually.
749
750			// System ID
751		✗	if (!system_id)
752			{
753		✗	system_id = current_messages.sysid;
754		✗	LOG_INFO("autopilot_interface.cpp - GOT VEHICLE SYSTEM ID: {}\n", system_id);
755			}
756
757			// Component ID
758		✗	if (!autopilot_id)
759			{
760		✗	autopilot_id = current_messages.compid;
761		✗	LOG_INFO("autopilot_interface.cpp - GOT AUTOPILOT COMPONENT ID: {}\n", autopilot_id);
762			}
763
764			// --------------------------------------------------------------------------
765			// GET INITIAL POSITION
766			// --------------------------------------------------------------------------
767
768			// Wait for initial position ned
769			/*
770			while (not(current_messages.time_stamps.local_position_ned && current_messages.time_stamps.attitude))
771			{
772			if (time_to_exit)
773			{
774			return;
775			}
776			usleep(500000);
777			}
778			*/
779			// copy initial position ned
780		✗	Mavlink_Messages local_data = current_messages;
781		✗	initial_position.x = local_data.local_position_ned.x;
782		✗	initial_position.y = local_data.local_position_ned.y;
783		✗	initial_position.z = local_data.local_position_ned.z;
784		✗	initial_position.vx = local_data.local_position_ned.vx;
785		✗	initial_position.vy = local_data.local_position_ned.vy;
786		✗	initial_position.vz = local_data.local_position_ned.vz;
787		✗	initial_position.yaw = local_data.attitude.yaw;
788		✗	initial_position.yaw_rate = local_data.attitude.yawspeed;
789
790		✗	LOG_INFO("autopilot_interface.cpp - INITIAL POSITION XYZ = [ {} , {} , {} ] \n", initial_position.x, initial_position.y, initial_position.z);
791		✗	LOG_INFO("autopilot_interface.cpp - INITIAL POSITION YAW = {} \n", initial_position.yaw);
792
793			// we need this before starting the write thread
794
795			// --------------------------------------------------------------------------
796			// WRITE THREAD
797			// --------------------------------------------------------------------------
798		✗	LOG_INFO("autopilot_interface.cpp - START WRITE THREAD \n");
799
800		✗	result = pthread_create(&write_tid, nullptr, &start_autopilot_interface_write_thread, this);
801		✗	if (result.has_value())
802			{
803		✗	LOG_INFO("result = {}", result);
804			}
805
806			// wait for it to be started
807		✗	while (!writing_status)
808			{
809		✗	usleep(100000); // 10Hz
810			}
811
812			// now we're streaming setpoint commands
813
814			// Done!
815		✗	}
816
817			// ------------------------------------------------------------------------------
818			// SHUTDOWN
819			// ------------------------------------------------------------------------------
820		✗	void Autopilot_Interface::stop()
821			{
822			// --------------------------------------------------------------------------
823			// CLOSE THREADS
824			// --------------------------------------------------------------------------
825		✗	LOG_INFO("autopilot_interface.cpp - CLOSE THREADS\n");
826
827			// signal exit
828		✗	time_to_exit = true;
829
830			// wait for exit
831		✗	pthread_join(read_tid, nullptr);
832		✗	pthread_join(write_tid, nullptr);
833
834			// now the read and write threads are closed
835
836			// still need to close the port separately
837		✗	}
838
839			// ------------------------------------------------------------------------------
840			// Read Thread
841			// ------------------------------------------------------------------------------
842		✗	void Autopilot_Interface::start_read_thread()
843			{
844		✗	if (reading_status != 0)
845			{
846		✗	LOG_WARN("autopilot_interface.cpp - read thread already running\n");
847			}
848			else
849			{
850		✗	read_thread();
851			}
852		✗	}
853
854			// ------------------------------------------------------------------------------
855			// Write Thread
856			// ------------------------------------------------------------------------------
857		✗	void Autopilot_Interface::start_write_thread()
858			{
859		✗	if (static_cast<bool>(writing_status))
860			{
861		✗	LOG_ERROR("autopilot_interface.cpp - write thread already running\n");
862			}
863			else
864			{
865		✗	write_thread();
866			}
867		✗	}
868
869			// ------------------------------------------------------------------------------
870			// Quit Handler
871			// ------------------------------------------------------------------------------
872		✗	void Autopilot_Interface::handle_quit(int /* sig */)
873			{
874		✗	disable_offboard_control();
875
876			try
877			{
878		✗	stop();
879			}
880		✗	catch (const std::exception& /* e */)
881			{
882		✗	LOG_ERROR("autopilot_interface.cpp - could not stop autopilot interface\n");
883		✗	}
884		✗	}
885
886			// ------------------------------------------------------------------------------
887			// Read Thread
888			// ------------------------------------------------------------------------------
889		✗	void Autopilot_Interface::read_thread()
890			{
891		✗	reading_status = true;
892
893		✗	while (!time_to_exit)
894			{
895		✗	read_messages();
896		✗	usleep(100000); // Read batches at 10Hz
897			}
898
899		✗	reading_status = false;
900		✗	}
901
902			// ------------------------------------------------------------------------------
903			// Write Thread
904			// ------------------------------------------------------------------------------
905		✗	void Autopilot_Interface::write_thread()
906			{
907			// signal startup
908		✗	writing_status = 2;
909			/*
910			// prepare an initial setpoint, just stay put
911			mavlink_set_position_target_local_ned_t sp;
912			sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY & MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE;
913			sp.coordinate_frame = MAV_FRAME_LOCAL_NED;
914			sp.vx = 0.0;
915			sp.vy = 0.0;
916			sp.vz = 0.0;
917			sp.yaw_rate = 0.0;
918
919			// set position target
920			{
921			std::lock_guard<std::mutex> lock(current_setpoint.mutex);
922			current_setpoint.data = sp;
923			}
924			*/
925			// write a message and signal writing
926			// write_setpoint();
927		✗	writing_status = true;
928			// Pixhawk needs to see off-board commands at minimum 2Hz,
929			// otherwise it will go into fail safe
930
931		✗	auto lastCallTimeGPS = std::chrono::system_clock::now();
932		✗	auto lastCallTimeMOCAP = std::chrono::system_clock::now();
933		✗	while (!time_to_exit)
934			{
935		✗	auto currentTime = std::chrono::system_clock::now();
936		✗	auto elapsedTimeGPS = std::chrono::duration_cast<std::chrono::duration<double>>(currentTime - lastCallTimeGPS).count();
937		✗	auto elapsedTimeMOCAP = std::chrono::duration_cast<std::chrono::duration<double>>(currentTime - lastCallTimeMOCAP).count();
938
939		✗	if ((GPS_Frequency > 0.0) && (elapsedTimeGPS >= 1.0 / GPS_Frequency))
940			{
941		✗	if (GPS_Active)
942			{
943		✗	GPS_Input(gps_lat_d, gps_lon_d, gps_alt, gps_vn, gps_ve, gps_vd, gps_time_week_ms, gps_time_week);
944			}
945		✗	lastCallTimeGPS = currentTime;
946			}
947		✗	if ((MOCAP_Frequency > 0.0) && (elapsedTimeMOCAP >= 1.0 / MOCAP_Frequency))
948			{
949		✗	if (MOCAP_Active)
950			{
951		✗	MOCAP_Input(mocap_w, mocap_x, mocap_y, mocap_z);
952			}
953		✗	lastCallTimeMOCAP = currentTime;
954			}
955			// usleep(100);
956			}
957			// signal end
958		✗	writing_status = false;
959		✗	}
960
961			// End Autopilot_Interface
962
963			// ------------------------------------------------------------------------------
964			// Pthread Starter Helper Functions
965			// ------------------------------------------------------------------------------
966
967		✗	void* start_autopilot_interface_read_thread(void* args)
968			{
969			// takes an autopilot object argument
970		✗	auto* autopilot_interface = static_cast<Autopilot_Interface*>(args);
971
972			// run the object's read thread
973		✗	autopilot_interface->start_read_thread();
974
975			// done!
976		✗	return nullptr;
977			}
978
979		✗	void* start_autopilot_interface_write_thread(void* args)
980			{
981			// takes an autopilot object argument
982		✗	auto* autopilot_interface = static_cast<Autopilot_Interface*>(args);
983
984			// run the object's read thread
985		✗	autopilot_interface->start_write_thread(); // NOLINT(clang-analyzer-core.CallAndMessage)
986
987			// done!
988		✗	return nullptr;
989			}
990
991			#endif
992