Autonomous flight#

There are three ways of implementing autonomous flight:

  1. Reuse Default mission Guidance modes: Move To / Relative move / FlightPlan

  2. Write a semi-custom Guidance mode inheriting from generic trajectory (TrajectoryFcamMode, TrajectoryStcamMode)

  3. Write a full-custom Guidance mode

1. Reuse Default mission Guidance modes#

You can use the following autonomous Guidance modes:

Move To: Give the drone a GPS destination coordinates and a target heading orientation.

Relative Move: Give the drone a distance to travel from current position and a yaw angle to reach from current yaw attitude.

FlightPlan: Give the drone a list of waypoints to reach. The waypoint list can be modified dynamically (How to update a FlightPlan dynamically).

Note

Waypoints can not be updated more than once per second. If a new waypoint update request is received faster than this, it will be delayed.

These three modes can be selected by using the set_guidance_mode() command. It includes trajectory correction by using built-in obstacle avoidance algorithms, which can be disabled if needed (using oa_manager).

2. Write a semi-custom Guidance mode inheriting from generic trajectory#

TrajectoryFcamMode from guidance::TrajectoryFcamMode class: Create a trajectory from drone 3D position and front camera 3D orientation inputs. Drone yaw angle and stereo camera orientation are automatically generated from the created trajectories. Obstacle avoidance can be activated to correct the generated trajectory.

TrajectoryStcamMode from guidance::TrajectoryStcamMode class: Create a trajectory from drone 3D position and stereo camera 3D orientation inputs. Drone yaw angle and front camera orientation are automatically generated from the created trajectories. Obstacle avoidance can’t be activated in this mode.

Trajectory points can be generated at a frequency up to 33 Hz in both modes.

It is required to call TrajectoryFcamMode or TrajectoryStcamMode base function at the beginning of each overridden method:

GdncModeExample::GdncModeExample(guidance::Guidance *guidance)
    : TrajectoryFcamMode(guidance, "com.company.missions.mymission.mymode"){}

void GdncModeExample::enter()
{
    TrajectoryFcamMode::enter();
    ...
}

void GdncModeExample::beginStep()
{
    TrajectoryFcamMode::beginStep();
    ...
}

void GdncModeExample::endStep()
{
    TrajectoryFcamMode::endStep();
    ...
}

void GdncModeExample::exit()
{
    TrajectoryFcamMode::exit();
    ...
}

Important

These mode templates limit the horizontal and vertical speed when the obstacle avoidance is enabled. Some methods are available to know the maximum horizontal, descent and ascent speeds:

TrajectoryFcamMode
TrajectoryStcamMode

3. Write a full-custom Guidance mode#

The drone 3D position, the front camera and the stereo camera 3D orientation trajectories are fully implemented by the developer (How to write a Guidance mode). Built-in obstacle avoidance can’t be used if you choose to write a full-custom guidance mode. However, occupancy grid and depthmap values can be used to implement your obstacle avoidance algorithm.

4. Tools to help writing a Guidance mode#

A guidance mode may use data types and utils implemented in libguidance-utils, for example Euler angles type, target reached detector, blockage detector, etc.

4.1 Types#

struct EulerAnglesZyx#

Container for ZYX Euler angles.

Public Functions

EulerAnglesZyx()#

Default constructor.

EulerAnglesZyx(float yaw, float pitch, float roll)#

Constructor.

EulerAnglesZyx nedToNedStart(float yawOffsetNedStartToNed) const#

Convert Euler angles relatively to the NED vector basis into Euler angles relatively to the NED-start vector basis.

WARNING The yaw of the output Euler angles in NED start is wrapped in ]-pi, pi]

EulerAnglesZyx nedStartToNed(float yawOffsetNedStartToNed) const#

Convert Euler angles relatively to the NED-start vector basis into Euler angles relatively to the NED vector basis.

WARNING The yaw of the output Euler angles in NED is wrapped in ]-pi, pi]

Public Members

float yaw#
float pitch#
float roll#
struct PositionTrajectoryPoint#

Container for a 3D position, a 3D velocity and a 3D acceleration.

Public Functions

PositionTrajectoryPoint()#
PositionTrajectoryPoint(const Eigen::Vector3f &position, const Eigen::Vector3f &velocity, const Eigen::Vector3f &acceleration)#
PositionTrajectoryPoint globalToLocal(float yawOffsetNedStartToNed, const Eigen::Vector2f &positionOffsetLocalToGlobal) const#

Convert the position trajectory point relatively to the global frame into a position trajectory point relatively to the local frame.

PositionTrajectoryPoint localToGlobal(float yawOffsetNedStartToNed, const Eigen::Vector2f &positionOffsetLocalToGlobal) const#

Convert the position trajectory point relatively to the local frame into a position trajectory point relatively to the global frame.

Public Members

Eigen::Vector3f position#
Eigen::Vector3f velocity#
Eigen::Vector3f acceleration#
struct GuidanceFdrLiteData#

Container for guidance user debug data.

Public Functions

inline GuidanceFdrLiteData()#
inline void setZero()#

Reset all values to zero.

Public Members

unsigned int solverItCount#
unsigned int numericalErrorCount#
float costComponents[COST_COMPONENTS_MAX]#
struct PilotingCommands#

Container for manual piloting command.

Public Functions

PilotingCommands()#
PilotingCommands(float rollAngleNed, float pitchAngleNed, float yawRateNed, float verticalVelocityNed)#
void clear()#

Public Members

float rollAngleNed#
float pitchAngleNed#
float yawRateNed#
float verticalVelocityNed#

4.2 Utils#

4.2.1 Target reached detectors#

Tools to detect drone/cameras target reached with respect to position, yaw and attitude.

class PositionReachedDetector#

Detect a reach with respect to position.

Subclassed by PositionReachedDetectorTest

Public Types

enum TargetKind#

Target kind for position reached detector, used to determine which attribute of target, state and which threshold will be taken into account to calculate target reached.

Values:

enumerator POSITION#
enumerator VELOCITY#
enumerator POSITION_VELOCITY#

Public Functions

PositionReachedDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

PositionReachedDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration

Parameters:

cfg – Configuration to apply.

void process(const PositionTrajectoryPoint &state)#

Calculate the distance between current state of drone and target.

Affect then the value of mTargetReached

Warning

All states have to be in same coordinate system (Cartesian)

Parameters:

state – Current local position trajectory point (position + velocity + acceleration) of drone

void reset()#

Reset isTargetReached to default value = false.

void setTarget(const PositionTrajectoryPoint &target)#

Set new target of position reached detector.

Parameters:

target – New target (position + velocity + acceleration)

void setThreshold(float threshold)#

Set new threshold of position reached detector. Used for the case kind = POSITION or VELOCITY, then the corresponding threshold will take value of threshold parameter.

Warning

Threshold must be zero or positive

Parameters:

threshold – New threshold (position or velocity threshold)

void setThreshold(float positionThreshold, float velocityThreshold)#

Set new threshold of position reached detector. Used for the case kind = POSITION_VELOCITY.

Warning

Thresholds must be zero or positive

Parameters:

  • positionThreshold – New position threshold

  • velocityThreshold – New velocity threshold

void setConfiguration(const Configuration &configuration)#

Set configuration of position reached detector.

Warning

Thresholds that are invalid are not set.

Parameters:

configuration – New configuration to use

bool isReached() const#

Know if drone reached its target.

Returns:

boolean indicator to indicate if drone reaches its target

struct Configuration#

Configuration for kind of target and threshold value for each target kind.

According to target kind, position detector will take the corresponding threshold:

  • If targetKind = POSITION, only positionTargetThreshold will be used

  • If targetKind = VELOCITY, only velocityTargetThreshold will be used

  • If targetKind = POSITION_VELOCITY, both positionTargetThreshold and velocityTargetThreshold will be used

Public Members

TargetKind targetKind#
float positionTargetThreshold#
float velocityTargetThreshold#
class YawReachedDetector#

Detect a reach with respect to yaw angle.

Subclassed by YawReachedDetectorTest

Public Types

enum TargetKind#

Values:

enumerator ANGLE#
enumerator ANGLE_RATE#

Public Functions

YawReachedDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

YawReachedDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration

Parameters:

cfg – Configuration to apply.

void process(float angleState, float rateState = 0.f, bool shouldWrap = true)#

Calculate the difference between current attitude yaw and target.

Affect then the value of mTargetReached

Yaw angle corresponds to the first Euler angle in the ZYX convention

Warning

All yaw angles must be express with respect to the same vector basis

Warning

To prevent singularities (gimbal lock), please refer to process method using quaternion as this method only assesses whether a state angle is close enough to a given target angle. This method does NOT prevent discontinuities in the case of Euler angles representation where the angle around the second axis in the decomposition does not lie in the range [-pi/2, pi/2[

Parameters:

  • angleState – Current angular state (yaw angle) [rad]

  • rateState – Current rate state. Default value = 0 [rad/s]

  • shouldWrap – Indicate if a wrap of a real value in ]-pi, pi] is needed. Default value = true

void process(const quaternion &quatState, float rateState = 0.f)#

Calculate the angular error between target and current state.

Affect then the value of mTargetReached

Warning

As state is quaternion, angles are wrapped automatically

Warning

Pitch and roll will not be verified from the quaternion

Parameters:

  • quatState – Current angular state (quaternion)

  • rateState – Current rate state. Default value = 0 [rad/s]

void reset()#

Reset isTargetReached to default value = false.

void setTarget(float target)#

Set new target of yaw reached detector.

Parameters:

target – New yaw angle target [rad]

void setThreshold(float threshold)#

Set new threshold of yaw reached detector.

Warning

Threshold must be zero or positive

Warning

Used when target kind = ANGLE

Parameters:

threshold – New yaw angle error threshold [rad]

void setThreshold(float angleThreshold, float rateThreshold)#

Set new thresholds of yaw reached detector.

Warning

Threshold must be zero or positive

Warning

Used when target kind = ANGLE_RATE

Parameters:

  • angleThreshold – New yaw angle error threshold [rad]

  • rateThreshold – New rate error threshold [rad/s]

void setConfiguration(const Configuration &cfg)#

Set configuration of yaw reached detector.

Warning

Thresholds that are invalid are not set.

Parameters:

cfg – New configuration to use

bool isReached() const#

Know if object reached its target.

Returns:

boolean indicator to indicate if object reaches its target

struct Configuration#

Configuration for kind of target and threshold value for each target kind.

Public Members

TargetKind targetKind#
float angleThreshold#
float rateThreshold#
class AttitudeReachedDetector#

Detect a reach with respect to attitude.

Subclassed by AttitudeReachedDetectorTest

Public Types

enum TargetKind#

Values:

enumerator ANGLE#
enumerator ANGLE_RATE#

Public Functions

AttitudeReachedDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

AttitudeReachedDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration

Parameters:

cfg – Configuration to apply.

void process(const quaternion &state)#

Calculate the difference between current attitude and target.

Affect then the value of mTargetReached

Parameters:

state – Current quaternion attitude [rad]

void reset()#

Reset isTargetReached to default value = false.

void setPeriod(float period)#

Set new period for attitude rate reached detector.

Parameters:

period – Period, delta time to calculate quaternion rate [s]

void setTarget(const quaternion &target)#

Set new target of attitude reached detector.

Parameters:

target – New quaternion target [rad]

void setThreshold(float threshold)#

Set new threshold of attitude reached detector.

Warning

Threshold must be zero or positive

Warning

Used when target kind = ANGLE

Parameters:

threshold – New attitude angle error threshold [rad]

void setThreshold(float angleThreshold, float rateThreshold)#

Set new thresholds of attitude reached detector.

Warning

Threshold must be zero or positive

Warning

Used when target kind = ANGLE_RATE

Parameters:

  • angleThreshold – New yaw angle error threshold [rad]

  • rateThreshold – New rate error threshold [rad/s]

void setConfiguration(const Configuration &cfg)#

Set configuration of attitude reached detector.

Warning

Thresholds that are invalid are not set.

Parameters:

cfg – New configuration to use

bool isReached() const#

Assess if current state has reached target.

Returns:

boolean indicator to indicate if object reaches its target

struct Configuration#

Configuration for kind of target and threshold value for each target kind.

Public Members

TargetKind targetKind#
float angleThreshold#
float rateThreshold#

4.2.2 Timeout detector#

A timeout detector increments an internal counter by a number of seconds each time it is processed, can be reset to zero, and indicates when its counter is greater than a given duration threshold.

class TimeoutDetector#

Detect a timeout after a number of update steps since being reset.

Public Functions

TimeoutDetector()#

Empty constructor.

User must call setConfiguration() before any other method.

TimeoutDetector(const Configuration &cfg)#

Constructor from configuration value.

Parameters:

cfg – Configuration values to apply.

void reset()#

Reset timer to zero.

void update()#

Update timer by the default sample period value.

See also

setSamplePeriod()

void update(float dt)#

Update timer by a custom amount of time in [s].

Parameters:

dt – increment of time in [s], not applied if negative.

bool isExpired()#

Indicate whether the timer is expired.

Returns:

true if calls to update() made the internal counter reach mConfiguration::timeoutDuration since last call to reset().

void setConfiguration(const Configuration &cfg)#

Adjust configuration values.

only meaningful values are changed, e.g. you can pass a negative samplePeriod to keep the previous samplePeriod.

Parameters:

cfg – new configuration to apply.

void setSamplePeriod(float period)#

Set default sample period.

only meaningful if the value is positive, otherwise it is not set.

Parameters:

period – New sample period in [s].

struct Configuration#

Configuration structure for a timeout.

Subclassed by guidance::AbstractBlockageDetector::Configuration

Public Members

float timeoutDuration#

Timeout in [s] after which isExpired() is true.

This value is not meaningful if negative. The timer expires if its internal timer is equal or greater than this value.

4.2.3 Motion detectors#

A motion detector keeps track of a reference value, a center point, and detects when the new values being processed are beyond a given threshold away from the center. Whenever this motion is detected, the last processed value becomes the new center point. There are different kinds of motion detectors with their own interpretation of “point” and “distance” (e.g. vectors, quaternion).

class AbstractMotionDetector#

Abstract base class for all motion detectors.

Subclassed by guidance::MotionDetector< T >, guidance::MotionDetector< Eigen::Vector3f >, guidance::MotionDetector< float >, guidance::MotionDetector< quaternion >

class AttitudeMotionDetector : public guidance::MotionDetector<quaternion>#

Detect motion with respect to attitude.

Public Functions

AttitudeMotionDetector() = default#

Default constructor.

The threshold is set to 0.0 initially.

AttitudeMotionDetector(float threshold)#

Constructor with a threshold configuration.

See also

setThresholdDistance()

Parameters:

threshold – distance at which a move is detected.

virtual void setReference(const quaternion &reference) override#

Set reference reference point to #reference.

template<typename T>
class MotionDetector : public guidance::AbstractMotionDetector#

Template class for MotionDetector for values of type T.

A move detector processes values of type T (see process()) and checks whether these values are in a given radius away from a reference point (see setReference()). When it is true, hasMoved() returns true, and the reference point is updated to the last point processed.

Public Functions

inline MotionDetector()#

Default constructor.

The threshold is set to 0.0 initially.

inline MotionDetector(float threshold)#

Constructor with a threshold configuration.

See also

setThresholdDistance()

Parameters:

threshold – distance at which a move is detected.

inline void reset()#

Reset detector.

inline virtual void setReference(const T &reference)#

Set reference reference point to #reference.

inline virtual void process(const T &reference)#

Process a new reference.

if a move is detected #reference becomes the new reference.

Parameters:

reference – new value to consider to check for a move.

inline bool hasMoved()#

Indicates whether the reference value has moved during the last call to process().

Returns:

True when the last processed point was too far from the reference value and became the new reference value.

inline void setThresholdDistance(float threshold)#

Set the threshold value.

The call does not update the threshold if the argument is negative.

Parameters:

threshold – Distance from reference at which a move is detected.

class PositionMotionDetector : public guidance::MotionDetector<Eigen::Vector3f>#

Detect motion with respect to a position.

Note

If the processed position is stuck in a given range for a given amount of time, a blockage is detected.

Public Functions

PositionMotionDetector() = default#

Default constructor.

The threshold is set to 0.0 initially.

PositionMotionDetector(float threshold)#

Constructor with a threshold configuration.

See also

setThresholdDistance()

Parameters:

threshold – distance at which a move is detected.

class YawMotionDetector : public guidance::MotionDetector<float>#

Detect motion with respect to yaw angle.

Public Functions

YawMotionDetector() = default#

Default constructor.

The threshold is set to 0.0 initially.

YawMotionDetector(float threshold)#

Constructor with a threshold configuration.

See also

setThresholdDistance()

Parameters:

threshold – distance at which a move is detected.

4.2.4 Blockage detectors#

A blockage detector combines one or more motion detectors with a timeout detector: it determines if a value being observed over time is blocked, which means that it did not move away from the motion detectors’ center points during a given amount of time.

class AbstractBlockageDetector#

Abstract class for all blockage detectors.

This class of detectors is used to detect when something hasn’t moved more than a given distance during a given time.

Subclassed by guidance::AttitudeBlockageDetector, guidance::PositionAttitudeBlockageDetector, guidance::PositionBlockageDetector, guidance::PositionYawBlockageDetector, guidance::YawBlockageDetector

Public Functions

AbstractBlockageDetector() = default#

Default constructor.

When using this constructor, the caller is expected to call setConfiguration before any other method.

AbstractBlockageDetector(const Configuration &cfg)#

Constructor from configuration.

See also

setConfiguration

Parameters:

cfg – Configuration object to apply.

void setConfiguration(const Configuration &cfg)#

Set configuration.

Some values in #cfg may not be valid: only the one that are valid are kept for configuring the detector, which means it is possible to only set one parameter by having the others be invalid.

Parameters:

cfg – Configuration to apply.

void setSamplePeriod(float period)#

Set default sample period.

only meaningful if the value is positive, otherwise it is not set.

Parameters:

period – New sample period in [s].

virtual void reset() = 0#

Reset detector state, this is defined in subclasses.

bool isBlocked() const#

Indicates if the detector detected a blockage or not.

Returns:

True if a blockage is being detected.

bool hasStateChanged() const#

Indicate this detector’s state has changed since last time the detector updated the state (see subclasses).

Typically a client first call hasStateChanged() before querying isBlocked(), to react only on value changes.

Returns:

True if the state of detector changed.

struct Configuration : public guidance::TimeoutDetector::Configuration#

Base class of configuration for blockage detectors.

Subclassed by guidance::AttitudeBlockageDetector::Configuration, guidance::PositionAttitudeBlockageDetector::Configuration, guidance::PositionBlockageDetector::Configuration, guidance::PositionYawBlockageDetector::Configuration, guidance::YawBlockageDetector::Configuration

class AttitudeBlockageDetector : public guidance::AbstractBlockageDetector#

Detect a blockage with respect to attitude.

If the processed attitude is stuck in a given range for a given amount of time, a blockage is detected.

Public Functions

AttitudeBlockageDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

AttitudeBlockageDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration()

Parameters:

cfg – Configuration to apply.

void setConfiguration(const Configuration &cfg)#

Set internal configuration.

If any value in the configuration is invalid, it won’t be applied to the internal configuration.

Parameters:

cfg – Configuration to apply.

virtual void reset() override#

Reset detector state, this is defined in subclasses.

void process(const quaternion &attitudeReference)#

Process a new attitude reference.

Parameters:

attitudeReference – New attitude point to check.

struct Configuration : public guidance::AbstractBlockageDetector::Configuration#

Configuration type for this detector.

Public Members

float attitudeThreshold#

threshold for quaternion-based motion detector. [rad]

class PositionAttitudeBlockageDetector : public guidance::AbstractBlockageDetector#

Detect a blockage with respect to position and attitude.

If both the processed altitude (quaternion) and position (Vector3f) are stuck in their respective ranges during a given amount of time, a blockage is detected.

Public Functions

PositionAttitudeBlockageDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

PositionAttitudeBlockageDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration()

Parameters:

cfg – Configuration to apply.

void setConfiguration(const Configuration &cfg)#

Set internal configuration.

If any value in the configuration is invalid, it won’t be applied to the internal configuration.

Parameters:

cfg – Configuration to apply.

virtual void reset() override#

Reset detector state, this is defined in subclasses.

void process(const Eigen::Vector3f &positionReference, const quaternion &attitudeReference)#

Process new position and attitude references.

Parameters:

  • positionReference – New position point to check.

  • attitudeReference – New attitude point to check.

struct Configuration : public guidance::AbstractBlockageDetector::Configuration#

Configuration type for this detector.

Public Members

float positionThreshold#

threshold for vector-based motion detector. [m]

float attitudeThreshold#

threshold for quaternion-based motion detector. [rad]

class PositionBlockageDetector : public guidance::AbstractBlockageDetector#

Detect a blockage with respect to a position.

If the processed position is stuck in a given range for a given amount of time, a blockage is detected.

Public Functions

PositionBlockageDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

PositionBlockageDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration()

Parameters:

cfg – Configuration to apply.

void setConfiguration(const Configuration &cfg)#

Set internal configuration.

If any value in the configuration is invalid, it won’t be applied to the internal configuration.

Parameters:

cfg – Configuration to apply.

virtual void reset() override#

Reset detector state, this is defined in subclasses.

void process(const Eigen::Vector3f &positionReference)#

Process a new position reference.

Parameters:

positionReference – New position point to check.

struct Configuration : public guidance::AbstractBlockageDetector::Configuration#

Configuration type for this detector.

Public Members

float positionThreshold#

threshold for vector-based motion detector. [m]

class PositionYawBlockageDetector : public guidance::AbstractBlockageDetector#

Detect a blockage with respect to position and yaw.

If both the processed yaw (float [rad]) and position (Vector3f) are stuck in their respective ranges during a given amount of time, a blockage is detected.

Public Functions

PositionYawBlockageDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

PositionYawBlockageDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration()

Parameters:

cfg – Configuration to apply.

void setConfiguration(const Configuration &cfg)#

Set internal configuration.

If any value in the configuration is invalid, it won’t be applied to the internal configuration.

Parameters:

cfg – Configuration to apply.

virtual void reset() override#

Reset detector state, this is defined in subclasses.

void process(const Eigen::Vector3f &positionReference, const float &yawReference)#

Process new position and yaw references.

Parameters:

  • positionReference – New position point to check.

  • yawReference – New yaw value to check.

struct Configuration : public guidance::AbstractBlockageDetector::Configuration#

Configuration type for this detector.

Public Members

float positionThreshold#

threshold for vector-based motion detector. [m]

float yawThreshold#

threshold for yaw motion detector. [rad]

class YawBlockageDetector : public guidance::AbstractBlockageDetector#

Detect a blockage with respect to an angle.

If the processed angle (float [rad]) is stuck in a given range for a given amount of time, a blockage is detected.

Public Functions

YawBlockageDetector() = default#

Default constructor with empty configuration.

Call setConfiguration() before any other method.

YawBlockageDetector(const Configuration &cfg)#

Constructor from configuration object.

See also

setConfiguration()

Parameters:

cfg – Configuration to apply.

void setConfiguration(const Configuration &cfg)#

Set internal configuration.

If any value in the configuration is invalid, it won’t be applied to the internal configuration.

Parameters:

cfg – Configuration to apply.

virtual void reset() override#

Reset detector state, this is defined in subclasses.

void process(const float &reference)#

Process a new angle reference.

Parameters:

reference – New angle to check.

struct Configuration : public guidance::AbstractBlockageDetector::Configuration#

Configuration type for this detector.

Public Members

float yawThreshold#

threshold for yaw motion detector. [m]

4.2.5 Absolute planner and framer#

To ensure as smooth movement of drone or cameras, these utilities can be used in guidance modes. Planner is used for vertical and horizontal translations and framer is used for camera and drone angles.

class AbsoluteFramer#

Framer to generate angular trajectory for guidance modes.

Public Types

enum AngleType#

Define angle orientation type.

Values:

enumerator NONE#
enumerator TARGET#
enumerator CUSTOM#

Public Functions

~AbsoluteFramer() = default#
void reset(float angleEst, float rateEst)#

Reset the framer to a given angle and rate.

To be called in the mode entry

Parameters:

  • angleEst – Estimated angle [rad]

  • rateEst – Estimated rate [rad/s]

void process()#

Update nominal trajectory to join the input angle state.

bool isYawTargetReached(const quaternion &quat, float rate = 0.f)#

Check if yaw destination is reached.

Warning

This function is only used for yaw

Parameters:

  • quat – State (in quaternion)

  • rate – Rate. Default value = 0 [rad/s]

Returns:

True if yaw destination angle is reached, false otherwise

bool isYawTargetReached(float angle, float rate = 0.f)#

Check if yaw destination is reached.

Warning

This function is only used for yaw

Parameters:

  • angle – Angle [rad]

  • rate – Rate. Default value = 0 [rad/s]

Returns:

True if yaw destination angle is reached, false otherwise

void setMaxAngleRef(float maxAngleRef)#

Set maximum angle reference [rad/s].

Parameters:

maxAngleRef – Maximum angle reference [rad/s]

void setMaxRateRef(float maxRateRef)#

Set maximum rate reference [rad/s].

Parameters:

maxRateRef – Maximum rate reference [rad/s]

void setAngle(AngleType angleType, float angleEst, float angleOffsetNedStartToNed, float fcamAngleInput, const guidance::PositionTrajectoryPoint &dronePositionStateInput, const guidance::PositionTrajectoryPoint &dronePositionStateEst)#

Set Angle.

Set angle type to NONE (constant angle), TARGET (heading towards destination), or CUSTOM (change angle to

Parameters:

  • angle), depending – on message from command handler

  • angleType – Angle type

  • angleEst – Estimated angle [rad]

  • angleOffsetNedStartToNed – NEDstart to NED angle offset [rad]

  • fcamAngleInput – Front camera angle input [rad]

  • dronePositionStateInput – Drone position input

  • dronePositionStateEst – Estimated drone position

void setRate(float rate)#

Set rate.

Parameters:

rate – Rate [rad/s]

const float &getAngleNom() const#

Return the nominal angular trajectory.

Note: The following getters return const references to be usable by telemetry producers

Returns:

Nominal angle [rad], rate [rad/s] and acceleration [rad/s^2]

const float &getRateNom() const#
const float &getAccelerationNom() const#
const float &getAngleTarget() const#

Get the Angle Target object.

Returns:

Target for yaw reached detector

Public Static Functions

static std::unique_ptr<AbsoluteFramer> create(const Configuration &config)#

Factory - config from configuration struct.

Produce a nullptr in case of invalid configuration file

Parameters:

config – Configuration

Returns:

Unique pointer to the framer

struct Configuration#

Configuration structure.

Public Members

physics::AngleIdealModel::Settings angleIdealModel#

angle ideal model settings

float angleThreshold#

angle reached detector validation angle threshold [rad]

float rateThreshold#

angle reached detector angle rate threshold [rad/s]

class AbsolutePlanner#

Planner to generate position trajectory for guidance modes.

Public Functions

~AbsolutePlanner()#
AbsolutePlanner() = delete#
AbsolutePlanner(const AbsolutePlanner&) = delete#
void reset(const guidance::PositionTrajectoryPoint &dronePositionStateEst)#

Reset the the planner to a given position, velocity and acceleration.

To be called in the mode entry

Parameters:

dronePositionStateEst – Drone estimate 3D position, velocity and acceleration

void process()#

Update nominal trajectory to join the input position state.

Parameters:

dronePositionStateInput – Drone input 3D position, velocity and acceleration

void processReachedDetector(const guidance::PositionTrajectoryPoint &dronePositionStateEst)#

Compute and update isReachedDetector flag.

void setDronePositionStateInput(const guidance::PositionTrajectoryPoint &dronePositionStateInput)#

Set destination position and velocity.

Parameters:

dronePositionStateInput – Position state input

void setMaxVerticalSpeed(float maxVerticalSpeed)#

Set maximum vertical speed [m/s].

The maximum speed is updated only if the input is non negative

void setMaxHorizontalSpeed(float maxHorizontalSpeed)#

Set maximum horizontal speed [m/s].

The maximum speed is updated only if the input is non negative

void setTargetReachedDetectorThreshold(float threshold)#

Update position reached detector threshold.

Warning

warning Threshold cannot be negative

Parameters:

threshold – Value of threshold

bool isDroneInputPositionReached() const#

Check if drone has reached destination position.

Returns:

True if destination position is reached, false otherwise

const guidance::PositionTrajectoryPoint &getDronePositionStateInput() const#

Return input position state.

Returns:

Drone input 3D position, velocity and acceleration

const guidance::PositionTrajectoryPoint &getDronePositionStateNom() const#

Return the nominal trajectory.

Returns:

Drone nominal 3D position, velocity and acceleration

Public Static Functions

static std::unique_ptr<AbsolutePlanner> create(const Configuration &config)#

Factory - config from configuration struct.

Produce a nullptr in case of invalid configuration file

Parameters:

Config – Configuration

Returns:

Unique pointer to the planner

struct Configuration#

Configuration structure.

Public Members

physics::HorizontalIdealModel::Settings horizontalIdealModel#

horizontal ideal model settings

physics::VerticalIdealModel::Settings verticalIdealModel#

vertical ideal model settings

float positionThreshold#

position reached detector position threshold (targetKind = POSITION) [m]

Both planner and framer need a configuration file (.cfg), with a physical ideal model and other settings, for example target reached’s threshold. There are already existing physics ideal model configuration files available in the drone file system:

  • /etc/guidance/common/physics_horizontal_ideal_model.cfg

  • /etc/guidance/common/physics_vertical_ideal_model.cfg

  • /etc/guidance/common/physics_yaw_ideal_model.cfg

It is possible to reuse the configuration files by creating symbolic links to common ideal models configuration files by these commands:

$ cd (MissionConfigDir)/etc/guidance/[YOUR_GUIDANCE_MODE_NAME]
$ ln -s /etc/guidance/common/physics_[horizontal/vertical/yaw]_ideal_model.cfg physics_[horizontal/vertical/yaw]_ideal_model.cfg

MissionConfigDir can be obtained by following the instructions here

These files can be imported in configuration of planner and framer.

Sample configuration file of planner (planner.cfg):

# Include horizontal ideal model
horizontal_ideal_model =
{
      @include "physics_horizontal_ideal_model.cfg"
};

# Include vertical ideal model
vertical_ideal_model =
{
      @include "physics_vertical_ideal_model.cfg"
};

# Position reached detector
position_reached_detector =
{
      positionThreshold = 1.0; # [m]
};

Sample configuration file of framer (framer.cfg):

# Include yaw ideal model
yaw_ideal_model =
{
      @include "physics_yaw_ideal_model.cfg"
};

# Yaw reached detector
yaw_reached_detector =
{
      angleThreshold = 0.1; # [rad]
      rateThreshold = 0.01; # [rad/s]
};

Theses files can then be retrieved from guidance modes using function getConfigDir() in C++ or get_config_dir() in Python.