Navigation

TODO: Short description of core modules and overall process, identical to the dev guide, probably refer the dev guide back here and then add extra notes

Core

class dronemanager.navigation.core.WayPointType(*values)

Bases: Enum

POS_NED = 1

POS_VEL_NED = 2

POS_VEL_ACC_NED = 3

VEL_NED = 4

VEL_BODY = 5

POS_GLOBAL = 6

class dronemanager.navigation.core.Waypoint(waypoint_type: WayPointType, pos: ndarray | None = None, vel: ndarray | None = None, acc: ndarray | None = None, gps: ndarray | None = None, yaw: float | None = None, yaw_rate: float | None = None)

Bases: object

property pos

property vel

property acc

property gps

property yaw

property yaw_rate

distance(other: Waypoint)

heading_ned(other: Waypoint)

heading_gps(other: Waypoint)

shift_gps(north: float, east: float, down: float) → Waypoint

Returns a new waypoint, offset by north, east and down from this waypoint. Note that the yaw is kept.

offset_gps(initial: Waypoint, target: Waypoint)

Creates a vector between two waypoints and then creates a new Waypoint offset from this waypoint by the same distance and heading.

class dronemanager.navigation.core.Fence(*args, **kwargs)

Bases: ABC

Abstract base class for geo-fence type classes and methods.

abstractmethod check_waypoint_compatible(point: Waypoint) → bool

Should return True if the waypoint fits within the fence, and false otherwise.

“Fits within” is taken broadly here, a fence can be inclusive, exclusive, around a dynamic obstacle, or

anything else. As long as True is returned when the waypoint is “good” and False otherwise, it works.

abstractmethod controller_safety(drone, forward, right, down, yaw, *args, **kwargs)

This function should adjust the controller inputs to prevent the drone from exceeding the fence.

Inputs are in the body frame of the drone. This function must be fast, expect it to be called at up to 100 Hz.

Parameters:

• drone

• forward

• right

• down

• yaw

• *args

• **kwargs

Returns:

The adjusted inputs as a tuple (forward, right, down, yaw)

abstract property bounding_box: ndarray

Should return an axis aligned bounding box for other components to use.

Output array should have shape (6,) and contain the limits as [north_lower, north_upper, east_lower, east_upper, down_lower, down_upper].

class dronemanager.navigation.core.PathGenerator(drone: Drone, logger, waypoint_type)

Bases: ABC

Abstract base class for path generators.

Parameters:

• drone

• logger

• waypoint_type

CAN_DO_GPS = False

WAYPOINT_TYPES = {}

These determine the type of intermediate waypoints a path generator may produce

target_position: Waypoint | None

set_target(waypoint: Waypoint)

Sets the target position that we will try to fly towards.

abstractmethod async create_path() → bool

Function that performs whatever calculations are initially necessary to be able to produce waypoints. This function may be quite slow.

abstractmethod next() → Waypoint

The next waypoint, once the follower algorithm asks for another one.

This function must execute quickly, as it might be called with a high frequency during flight. Path followers should call it when they think they have reached the current waypoint. Should return None if the path generator hasn’t produced any waypoints or has run out.

class dronemanager.navigation.core.PathFollower(drone: Drone, logger, dt, setpoint_type: WayPointType)

Bases: ABC

Abstract Base class to “follow” a given path and maintain position at waypoints.

A path follower can work with different types of waypoints, but must be able to process WayPoinType.POS_NED, as that is the default case when a generator isn’t providing waypoints.

CAN_DO_GPS = False

SETPOINT_TYPES = {}

WAYPOINT_TYPES = {}

current_waypoint: Waypoint | None

activate()

async deactivate()

property is_active

async follow()

Follows waypoints produced from a path generator by sending setpoints to the drone FC.

Requests a new waypoint from the PathGenerator when get_next_waypoint returns True. If the PG does not produce a waypoint, holds position instead.

Returns:

abstractmethod get_next_waypoint() → bool

Function that determines when to get the next waypoint from the path generator.

PathGenerator.next() is called during the follow loop when this function returns True. It should always return True if we don’t have a waypoint already.

abstractmethod async set_setpoint(waypoint)

Function that determines the next setpoint required to get to the target waypoint. This function is called once every dt seconds using either the next waypoint from the path generator or the drones current position.

Returns:

close()

Path Generators

class dronemanager.navigation.directtargetgenerator.DirectTargetGenerator(drone: Drone, logger, waypoint_type, *args, **kwargs)

Bases: PathGenerator

Simply sends the target waypoint as static setpoints.

Parameters:

• drone

• logger

• waypoint_type

CAN_DO_GPS = True

WAYPOINT_TYPES = {WayPointType.POS_GLOBAL, WayPointType.POS_NED}

These determine the type of intermediate waypoints a path generator may produce

async create_path()

Function that performs whatever calculations are initially necessary to be able to produce waypoints. This function may be quite slow.

next()

Returns the target position exactly once and then None.

GMP3

class dronemanager.navigation.gmp3generator.GMP3Generator(drone, dt, logger)

Bases: PathGenerator

Parameters:

• drone

• logger

• waypoint_type

WAYPOINT_TYPES = {WayPointType.POS_VEL_NED}

These determine the type of intermediate waypoints a path generator may produce

CAN_DO_GPS = False

async create_path()

Function that performs whatever calculations are initially necessary to be able to produce waypoints. This function may be quite slow.

next() → Waypoint | None

The next waypoint, once the follower algorithm asks for another one.

This function must execute quickly, as it might be called with a high frequency during flight. Path followers should call it when they think they have reached the current waypoint. Should return None if the path generator hasn’t produced any waypoints or has run out.

class dronemanager.navigation.GMP3.GMP3Config(maxit, alpha, wdamp, delta, vx_max, vy_max, vz_max, Q11, Q22, Q33, Q12, Q13, Q23, dt, x_max, x_min, y_max, y_min, z_max, z_min, obstacles=None)

Bases: object

Obstacles: list of tuples (x, y, z, r)

class dronemanager.navigation.GMP3.GMP3(gmpConfig: GMP3Config)

Bases: object

enforce_bounds(x, y, z)

Cost1(xobs, yobs, zobs, robs, x_in, y_in, z_in, x_f1, x_f2, y_f1, y_f2, z_f1, z_f2, theta)

Cost(xobs, yobs, zobs, robs, x_in, y_in, z_in, x_f1, x_f2, y_f1, y_f2, z_f1, z_f2, theta)

Agents(xobs, yobs, zobs, robs, x_in, y_in, z_in, x_f1, x_f2, y_f1, y_f2, z_f1, z_f2, theta, delta, n)

results(x_in, y_in, z_in, x_f1, x_f2, y_f1, y_f2, z_f1, z_f2, theta)

calculate(startposition, finalposition)

Path Followers

class dronemanager.navigation.directsetpointfollower.DirectSetpointFollower(drone: Drone, logger, dt, setpoint_type, *args, **kwargs)

Bases: PathFollower

CAN_DO_GPS = True

SETPOINT_TYPES = {WayPointType.POS_GLOBAL, WayPointType.POS_NED, WayPointType.POS_VEL_ACC_NED, WayPointType.POS_VEL_NED, WayPointType.VEL_BODY, WayPointType.VEL_NED}

WAYPOINT_TYPES = {WayPointType.POS_GLOBAL, WayPointType.POS_NED, WayPointType.POS_VEL_ACC_NED, WayPointType.POS_VEL_NED, WayPointType.VEL_BODY, WayPointType.VEL_NED}

get_next_waypoint() → bool

Function that determines when to get the next waypoint from the path generator.

PathGenerator.next() is called during the follow loop when this function returns True. It should always return True if we don’t have a waypoint already.

async set_setpoint(waypoint)

Function that determines the next setpoint required to get to the target waypoint. This function is called once every dt seconds using either the next waypoint from the path generator or the drones current position.

Returns:

class dronemanager.navigation.ruckigfollower.RuckigOfflineFollower(drone: Drone, logger, dt: float, setpoint_type, max_vel=10.0, max_acc=2.0, max_jerk=1.0, max_down_vel=1.0, max_up_vel=3.0, max_v_acc=0.5, max_v_jerk=1.0, max_yaw_vel=60, max_yaw_acc=30, max_yaw_jerk=30)

Bases: PathFollower

CAN_DO_GPS = True

SETPOINT_TYPES = {WayPointType.POS_VEL_ACC_NED}

WAYPOINT_TYPES = {WayPointType.POS_GLOBAL, WayPointType.POS_NED, WayPointType.POS_VEL_ACC_NED, WayPointType.POS_VEL_NED}

activate()

async deactivate()

get_next_waypoint() → bool

Function that determines when to get the next waypoint from the path generator.

PathGenerator.next() is called during the follow loop when this function returns True. It should always return True if we don’t have a waypoint already.

async set_setpoint(waypoint)

Function that determines the next setpoint required to get to the target waypoint. This function is called once every dt seconds using either the next waypoint from the path generator or the drones current position.

Returns:

class dronemanager.navigation.ruckigfollower.RuckigOnlineFollower(drone: Drone, logger, dt: float, setpoint_type, max_vel=10.0, max_acc=2.0, max_jerk=1.0, max_v_vel=1.0, max_v_acc=0.5, max_v_jerk=1.0)

Bases: PathFollower

CAN_DO_GPS = False

SETPOINT_TYPES = {WayPointType.POS_VEL_ACC_NED}

WAYPOINT_TYPES = {WayPointType.POS_NED, WayPointType.POS_VEL_ACC_NED, WayPointType.POS_VEL_NED}

activate()

async deactivate()

get_next_waypoint() → bool

Function that determines when to get the next waypoint from the path generator.

PathGenerator.next() is called during the follow loop when this function returns True. It should always return True if we don’t have a waypoint already.

async set_setpoint(waypoint)

Function that determines the next setpoint required to get to the target waypoint. This function is called once every dt seconds using either the next waypoint from the path generator or the drones current position.

Returns:

class dronemanager.navigation.velocityfollower.VelocityFollower(drone: Drone, logger, dt, max_vel_h=1.0, max_vel_z=0.5, max_acc_h=0.5, max_acc_z=0.25, max_yaw_rate=60)

Bases: PathFollower

Flies directly toward the waypoint facing towards it along the way. Turning towards the target yaw happens after we reach the waypoint. Control happens only through velocity setpoints.

Currently very WIP, drifts off as soon as target positions are reached.

SETPOINT_TYPES = {WayPointType.VEL_NED}

WAYPOINT_TYPES = {WayPointType.POS_NED}

CAN_DO_GPS = False

get_next_waypoint() → bool

Function that determines when to get the next waypoint from the path generator.

PathGenerator.next() is called during the follow loop when this function returns True. It should always return True if we don’t have a waypoint already.

async set_setpoint(waypoint)

Always move towards target. Accelerates if we are slower than the max speed and have space to accelerate, keep speed if we are at max velocity and still some distance away from target, decelerate when we approach target.

Returns:

Fences

TODO: Short guide on the various geometries. Fence limits, fence limits with drone size accounted for, extra safety margin based on safety level Key functions: “display boundary”, “stick safety”

class dronemanager.navigation.rectlocalfence.RectLocalFence(north_lower, north_upper, east_lower, east_upper, down_lower, down_upper, safety_level=0)

Bases: Fence

Class for rectangular fences in the local coordinate frame.

Works by defining yis limits: One upper and lower limit for each axis. Waypoints will only be accepted if they use local (NED) coordinates and lie within the box between these limits. Lower limits but be smaller than upper limits. Safety level is used only by the controller functionality.

check_waypoint_compatible(point: Waypoint)

Should return True if the waypoint fits within the fence, and false otherwise.

“Fits within” is taken broadly here, a fence can be inclusive, exclusive, around a dynamic obstacle, or

anything else. As long as True is returned when the waypoint is “good” and False otherwise, it works.

controller_safety(drone, forward_input, right_input, vertical_input, yaw_input, *args, **kwargs)

This function should adjust the controller inputs to prevent the drone from exceeding the fence.

Inputs are in the body frame of the drone. This function must be fast, expect it to be called at up to 100 Hz.

Parameters:

• drone

• forward

• right

• down

• yaw

• *args

• **kwargs

Returns:

The adjusted inputs as a tuple (forward, right, down, yaw)

property bounding_box: ndarray

Should return an axis aligned bounding box for other components to use.

Output array should have shape (6,) and contain the limits as [north_lower, north_upper, east_lower, east_upper, down_lower, down_upper].