Control_functions module

Contents

This module is designed to make high level control programming more simple.

Classes

struct control_api_waypoint

Functions

void set_heading(float heading)
void set_destination(float x, float y, float z, float psi)
auto wait4connect() -> int
auto wait4start() -> int
auto initialize_local_frame() -> int
auto takeoff(float takeoff_alt) -> int
auto check_waypoint_reached() -> int
auto land() -> int
auto init_publisher_subscriber(ros::NodeHandle controlnode) -> int

Function documentation

void set_heading(float heading)

Returns n/a

This function is used to specify the drone’s heading in the local reference frame. Psi is a counter clockwise rotation following the drone’s reference frame defined by the x axis through the right side of the drone with the y axis through the front of the drone.

void set_destination(float x, float y, float z, float psi)

Returns n/a

This function is used to command the drone to fly to a waypoint. These waypoints should be specified in the local reference frame. This is typically defined from the location the drone is launched. Psi is counter clockwise rotation following the drone’s reference frame defined by the x axis through the right side of the drone with the y axis through the front of the drone.

int wait4connect()

Returns 0 - connected to fcu

Wait for connect is a function that will hold the program until communication with the FCU is established.

int wait4start()

Returns 0 - mission started

Wait for strat will hold the program until the user signals the FCU to enther mode guided. This is typically done from a switch on the safety pilot’s remote or from the ground control station.

int initialize_local_frame()

Returns 0 - frame initialized

This function will create a local reference frame based on the starting location of the drone. This is typically done right before takeoff. This reference frame is what all of the the set destination commands will be in reference to.

int takeoff(float takeoff_alt)

Returns 0 - nominal takeoff

The takeoff function will arm the drone and put the drone in a hover above the initial position.

int check_waypoint_reached()

Returns 1 - waypoint reached

This function returns an int of 1 or 0. THis function can be used to check when to request the next waypoint in the mission.

int land()

Returns 1 - mode change successful

this function changes the mode of the drone to land

int init_publisher_subscriber(ros::NodeHandle controlnode)

Returns n/a

This function is called at the beginning of a program and will start of the communication links to the FCU. The function requires the program's ros nodehandle as an input