#include <atomic>
#include <iomanip>
#include <iostream>
#include <unistd.h>
#include <thread>
#include <vector>
#include "api/flight_control_interface.hpp"

Functions
std::atomic_bool	gMissionInProgress (true)

void	mission_task ()
	Task to command the vehicle to execute a waypoint mission. More...

void	telemetry_task ()
	Task to receive and display telemetry data. More...

void	frame_update_task ()
	Task to update the transform between snav_fci::ReferenceFrame::WAYPOINT and its parent. More...

int	main (int argc, char *argv[])
	This is an example of a multithreaded waypoint-following mission. More...

Function Documentation

void frame_update_task ( )

Task to update the transform between snav_fci::ReferenceFrame::WAYPOINT and its parent.

In this example, the transform is updated using the transform to snav_fci::ReferenceFrame::LAUNCH, which makes the mission relative to the takeoff position and orientation. This same behavior can be achieved simply by setting snav_fci::TxConfig::waypoint_frame_parent to snav_fci::ReferenceFrame::LAUNCH, but this example is intended to demonstrate how to use the snav_fci::FlightControlInterface::set_waypoint_frame_tf() function. A typical use of this function would be to update snav_fci::ReferenceFrame::WAYPOINT using some form of global localization, such as AprilTags, in order to navigate in a global frame.

int main	(	int	argc,
		char *	argv[]
	)

This is an example of a multithreaded waypoint-following mission.

It also does a better job of checking return codes provided by snav_fci::FlightControlInterface than basic_waypoint_example.cpp.

This example uses three threads:

mission_thread: commands the vehicle to go to waypoints
frame_update_thread: updates the transform between ReferenceFrame::WAYPOINT and its parent such that the mission is relative to the launch position and orientation; note that this is just intended to serve as an example of using the snav_fci::FlightControlInterface::set_waypoint_frame_tf() function and would normally by achieved by simply using snav_fci::TxConfig::waypoint_frame_parent
telemetry_thread: retrieves data from Qualcomm Navigator and prints it to stdout

A similar multithreaded architecture may be suitable for a host of other use cases and is more easily achieved using snav_fci::FlightControlInterface compared to using the Qualcomm Navigator API directly.

void mission_task ( )

Task to command the vehicle to execute a waypoint mission.

This thread blocks while the vehicle executes the commanded maneuvers.

Configure snav_fci::FlightControlInterface in this thread since this object has snav_fci::FlightControlInterface::Permissions::READ_WRITE and will launch both the tx and rx threads if not already launched. Other threads will wait for this to happen before calling snav_fci::FlightControlInterface::connect() using the snav_fci::FlightControlInterface::wait_for_configure() function.

Take advantage of snav_fci::TakeoffConfig to configure the takeoff. Set take off height to 2 m, min takeoff speed to 0.05 m/s, max takeoff speed to 1 m/s, and max linear acceleration magnitude to 0.25 m/s^2.

Define the waypoints for this mission, keeping in mind that the vehicle is at (0, 0, 2) m after takeoff phase ends:

Go to (4, 0, 2) m
Go to (4, 4, 2) m
Go to (0, 4, 2) m
Go to (0, 0, 2) m (back to start)

The result is a square in XY plane with constant Z.

void telemetry_task ( )

Task to receive and display telemetry data.

This thread loops at a slow rate and prints useful telemetry data without interfering with the more critical mission_task thread.

Functions

Function Documentation