Autonomous flight#
There are three ways of implementing autonomous flight:
Reuse Default mission Guidance modes: Move To / Relative move / FlightPlan
Write a semi-custom Guidance mode inheriting from generic trajectory (TrajectoryFcamMode, TrajectoryStcamMode)
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]
-
EulerAnglesZyx()#
-
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.
-
PositionTrajectoryPoint()#
-
struct GuidanceFdrLiteData#
Container for guidance user debug data.
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
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
- 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
-
PositionReachedDetector() = default#
-
class YawReachedDetector#
Detect a reach with respect to yaw angle.
Subclassed by YawReachedDetectorTest
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
- 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.
-
YawReachedDetector() = default#
-
class AttitudeReachedDetector#
Detect a reach with respect to attitude.
Subclassed by AttitudeReachedDetectorTest
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
- 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.
-
AttitudeReachedDetector() = default#
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
-
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.
-
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.
-
float timeoutDuration#
-
TimeoutDetector()#
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
- Parameters:
threshold – distance at which a move is detected.
-
virtual void setReference(const quaternion &reference) override#
Set reference reference point to #reference.
-
AttitudeMotionDetector() = default#
-
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
- Parameters:
threshold – distance at which a move is detected.
-
inline void reset()#
Reset detector.
-
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.
-
inline MotionDetector()#
-
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.
-
class YawMotionDetector : public guidance::MotionDetector<float>#
Detect motion with respect to yaw angle.
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
- 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
-
AbstractBlockageDetector() = default#
-
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
- 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]
-
float attitudeThreshold#
-
AttitudeBlockageDetector() = default#
-
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
- 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.
-
PositionAttitudeBlockageDetector() = default#
-
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
- 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]
-
float positionThreshold#
-
PositionBlockageDetector() = default#
-
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
- 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.
-
PositionYawBlockageDetector() = default#
-
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
- 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]
-
float yawThreshold#
-
YawBlockageDetector() = default#
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
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.
-
~AbsoluteFramer() = default#
-
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.
-
~AbsolutePlanner()#
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.