Guidance API#

Enumerations#

enum guidance::Trigger#

A Guidance Mode can specify at which rate their step() method should be called. This is done through the guidance::Mode::getTriggers method, which returns a timeout T, a period N and a bitmask build from the following values. Each enum value correspond to a possible event that triggers the step() method. TRIGGER_TIMER and TRIGGER_TICK cannot both be set.

Values:

enumerator TRIGGER_TIMER#

Fired after T ms after the last trigger, if no other trigger happen

enumerator TRIGGER_TICK#

Every N ticks as computed by Drone Controller

enumerator TRIGGER_DEPTH_MAP#

A new depth map is computed

enum guidance::ChannelKind#

Each enum value identifies one predefined Channel

Values:

enumerator CHANNEL_KIND_GUIDANCE#

Server channel (normally the client is Flight Supervisor)

enumerator CHANNEL_KIND_DRONE_CONTROLLER#

Client channel connected to Drone Controller.

enum guidance::SenderKind#

Each enum identifies a Sender object, i.e. an interface through which protobuf messages can be sent (see libmsghub).

Values:

enumerator SENDER_KIND_DRONE_CONTROLLER#
enumerator SENDER_KIND_FRONT_CAM_CONTROLLER#
enumerator SENDER_KIND_FRONT_ZOOM_CAM_CONTROLLER#
enumerator SENDER_KIND_STEREO_CAM_CONTROLLER#
enumerator SENDER_KIND_GUIDANCE#

Classes#

class Guidance#

Guidance interface

This is the Guidance interface as seen from modes.

Subclassed by guidance::GuidanceImpl

Public Functions

inline Guidance()#
inline virtual ~Guidance()#
virtual pomp::Loop *getLoop() = 0#

Return the POMP loop owned by this guidance instance. This event base is used to aggregate events from external and internal messages (see libpomp.hpp).

virtual msghub::MessageHub *getMessageHub() = 0#

Return the current MessageHub instance, which is responsible for managing channels, handlers and senders for protocol buffer messages (see libmsghub).

virtual msghub::Channel *getChannel(ChannelKind kind) = 0#

Get the instance of Channel associated with an identifier

Parameters:

kind – identifies the kind of channel to access.

virtual msghub::MessageSender *getSender(SenderKind kind) = 0#

Get the MessageSender associated with an identifier.

Parameters:

kind – identifies the kind of sender to access.

virtual cfgreader::ConfigReader *getConfig() = 0#

Get the current ConfigReader instance, which holds static configuration values for the current drone hardware.

virtual const SpatialPerception *getSpatialPerception() const = 0#

Get the SpatialPerception instance owned by the guidance object. This gives access to the occupancy grid and the spherical map.

virtual bool addMode(Mode *mode) = 0#

Register a Mode The mode’s name is used as a key to identify it. Two modes cannot have the same name. If the name already exists, the current mode is NOT replaced by the new one. The mode name cannot be empty, or equal to “null”.

Parameters:

mode – non-null pointer to a Mode instance

Returns:

true if the operation succeeded.

virtual bool removeMode(Mode *mode) = 0#

Unregister a Mode

Parameters:

mode – non-null pointer to a Mode instance

Returns:

true if the mode was registered and is successfully unregistered.

class Input#

Guidance input

The active mode has access to an instance of Input, which holds the current inputs at each step.

Public Types

using Pcmd = ::Guidance::Messages::Pcmd#

Public Functions

inline Input()#
inline void clearDepthMap()#

Clear members

inline void clearPcmds()#

Public Members

const struct vipc_frame *mDepthMap#
Pcmd mPcmd#
class Output#

Guidance output

At each step of execution the active mode can update an instance of Output, which holds different drone and camera references expressed as protobuf messages.

Each output is associated with a boolean flag for that output that indicates if the output is present at this step or not. At the beginning of a step, all flags are false.

Public Types

using HorizontalReference = DroneController::Messages::HorizontalReference#
using VerticalReference = DroneController::Messages::VerticalReference#
using YawReference = DroneController::Messages::YawReference#
using FrontCamReference = AxesCamController::Messages::Reference#
using FrontCamZoom = ZoomCamController::Messages::Zoom#
using StereoCamReference = AxesCamController::Messages::Reference#
using FrontCamReferenceConfig = AxesCamController::Messages::Config#
using FrontCamZoomConfig = ZoomCamController::Messages::Config#
using StereoCamReferenceConfig = AxesCamController::Messages::Config#
using VerticalPositionTarget = ColibryLite::Messages::VerticalPositionTarget#
using VerticalVelocityTarget = ColibryLite::Messages::VerticalVelocityTarget#
using HorizontalPositionTarget = ColibryLite::Messages::HorizontalPositionTarget#
using HorizontalVelocityTarget = ColibryLite::Messages::HorizontalVelocityTarget#
using YawPositionTarget = ColibryLite::Messages::YawPositionTarget#
using YawVelocityTarget = ColibryLite::Messages::YawVelocityTarget#

Public Functions

inline Output()#
inline void clear()#

Clear members

Public Members

HorizontalReference mHorizontalReference#
bool mHasHorizontalReference#
VerticalReference mVerticalReference#
bool mHasVerticalReference#
YawReference mYawReference#
bool mHasYawReference#
FrontCamReference mFrontCamReference#
bool mHasFrontCamReference#
FrontCamZoom mFrontCamZoom#
bool mHasFrontCamZoom#
StereoCamReference mStereoCamReference#
bool mHasStereoCamReference#
FrontCamReferenceConfig mFrontCamReferenceConfig#
bool mHasFrontCamReferenceConfig#
FrontCamZoomConfig mFrontCamZoomConfig#
bool mHasFrontCamZoomConfig#
StereoCamReferenceConfig mStereoCamReferenceConfig#
bool mHasStereoCamReferenceConfig#
VerticalPositionTarget mVerticalPositionTarget#
bool mHasVerticalPositionTarget#
VerticalVelocityTarget mVerticalVelocityTarget#
bool mHasVerticalVelocityTarget#
HorizontalPositionTarget mHorizontalPositionTarget#
bool mHasHorizontalPositionTarget#
HorizontalVelocityTarget mHorizontalVelocityTarget#
bool mHasHorizontalVelocityTarget#
YawPositionTarget mYawPositionTarget#
bool mHasYawPositionTarget#
YawVelocityTarget mYawVelocityTarget#
bool mHasYawVelocityTarget#
uint32_t mTickNum#
class Mode#

Guidance Mode

At each step (with registered triggers) the methods will be called in this order:

  • beginStep

  • generateDroneReference

  • correctDroneReference

  • generateAttitudeReferences

  • endStep

Subclassed by AbsoluteMoveMode, CameraRotationMoveMode, CarpetMode, DescentMode, FlightPlanMode, guidance::NullMode, guidance::TrajectoryFcamMode, guidance::TrajectoryStcamMode, LookAtMode, ManualMode, PoiMode

Public Functions

inline Mode(Guidance *guidance)#
inline virtual ~Mode()#
virtual const std::string &getName() = 0#

Get the name of the guidance mode (shall be unique in the system).

virtual void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period) = 0#

Get the triggers for the mode

Parameters:
  • triggers – mask of Trigger values. TRIGGER_TICK can not be combined with TRIGGER_TIMER.

  • timeout – if no frames received during this time (in ms), force a step.

  • period – if a timer is required, its period (in ms). If there is also a trigger on frame with a timeout specified, it will only be called when there is no frame. For tick trigger, this is the period of wakeup measured in ticks of the Drone Controller. For a 200Hz tick in drone controller this means a increment of 5ms per tick.

inline virtual void configure(const ::google::protobuf::Any &config, bool disableObstacleAvoidance, bool override_front_camera, bool override_stereo_camera)#

Called when the mode is set with a configuration

inline virtual bool hasObstacleAvoidance()#

Return whether the mode implements obstacle avoidance Unless this function is overriden, modes do not advertise they implement obstacle avoidance. TODO: Add to mode descriptor

inline virtual void enter()#

Called when the mode is activated and become the current one.

inline virtual void exit()#

Called when the mode is de-activated.

inline virtual void beginStep()#

Start a new step of the guidance The input data is valid at this point

inline virtual void endStep()#

Finish a step of the guidance Output data shall be valid after this point

inline virtual void generateDroneReference()#

Generate the drone trajectory reference (horizontal and vertical).

inline virtual void correctDroneReference()#

Correct the generated trajectory using obstacle avoidance data.

inline virtual void generateAttitudeReferences()#

Generate the front camera, stereo camera and drone attitudes references. The camera references do not depend on the drone ones, but will be saturated if not reachable.

inline const Input *getInput() const#
inline Output *getOutput()#
inline void setError(const std::string &message)#

Report an error in this mode. The error message is logged and sent back to the Flight Supervisor. Guidance will call the exit() method (in case we are not already in exit()), and deactivate the current mode.

inline Guidance *getGuidance()#
class SpatialPerception#

Subclassed by guidance::SpatialPerceptionImpl

Public Types

typedef Eigen::Matrix<float, 3, -1, Eigen::RowMajor> PointsVector#

Public Functions

virtual struct moser_occupancy_grid *getOccupancyGrid() const = 0#
virtual struct moser_spherical_map *getStereoSphericalMap() const = 0#
virtual void publishMiscDebugData(const PointsVector &nominalTraj, const std::array<float, 4> &nominalTrajColor, const PointsVector &referenceTraj, const std::array<float, 4> &referenceTrajColor) const = 0#
class TrajectoryFcamMode : public guidance::Mode#

Subclassed by RelativeMoveMode

Public Functions

TrajectoryFcamMode(Guidance *guidance, const std::string &name)#
virtual ~TrajectoryFcamMode() override#
void setDroneNominalPositionTrajectoryGlobal(const guidance::PositionTrajectoryPoint &point)#

Set nominal global drone trajectory (to be called from generateDroneReference) Will be adjusted in correctDroneReference if obstable avoidance is enabled.

void setDroneNominalPositionTrajectoryLocal(const guidance::PositionTrajectoryPoint &point)#

Set nominal local drone trajectory (to be called from generateDroneReference) Will be adjusted in correctDroneReference if obstable avoidance is enabled

void setFcamEulerAnglesZyxNed(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setFcamEulerAnglesZyxNedStart(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED Start

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

virtual void generateFcamAttitudeReference() = 0#

Called at the beginning of generateAttitudeReference to let derived class to fill mFcamEulerAnglesNedStartRef, mFcamEulerAnglesDerivativesNedStartRef, mFcamAcceleration and mFcamZoom.

virtual const guidance::PositionTrajectoryPoint &processObstacleAvoidance()#

Process obstacle avoidance algorithm. This method is usually called by correctDroneReference. It can be overrided to change obstacle avoidance behavior.

void enableObstacleAvoidance(bool enable)#

Enable or disable obstacle avoidance

Parameters:

enable – true to enable obstacle avoidance, false otherwise

bool isObstacleAvoidanceEnabled()#

Get obstacle avoidance status

Returns:

True if obstacle avoidance is enabled

inline uint32_t getTickPeriod() const#
inline float getTimePeriod() const#
inline const PositionTrajectoryPoint &getDroneNominalPositionTrajectoryLocal() const#
inline const PositionTrajectoryPoint &getDroneReferencePositionTrajectoryLocal() const#
float getMaxHorizontalSpeed() const#

Return the maximum admissible horizontal speed [m/s] (> 0)

float getMaxDescentSpeed() const#

Return the maximum admissible descent speed [m/s] (> 0)

float getMaxAscentSpeed() const#

Return the maximum admissible ascent speed [m/s] (> 0)

virtual bool hasObstacleAvoidance() override#

Return whether the mode implements obstacle avoidance Unless this function is overriden, modes do not advertise they implement obstacle avoidance. TODO: Add to mode descriptor

virtual void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period) override#

Get the triggers for the mode

Parameters:
  • triggers – mask of Trigger values. TRIGGER_TICK can not be combined with TRIGGER_TIMER.

  • timeout – if no frames received during this time (in ms), force a step.

  • period – if a timer is required, its period (in ms). If there is also a trigger on frame with a timeout specified, it will only be called when there is no frame. For tick trigger, this is the period of wakeup measured in ticks of the Drone Controller. For a 200Hz tick in drone controller this means a increment of 5ms per tick.

virtual void configure(const ::google::protobuf::Any &config, bool disableObstacleAvoidance, bool overrideFrontCamera, bool overrideStereoCamera) override#

Called when the mode is set with a configuration

virtual void enter() override#

Called when the mode is activated and become the current one.

virtual void beginStep() override#

Start a new step of the guidance The input data is valid at this point

virtual void correctDroneReference() override#

Correct the generated trajectory using obstacle avoidance data.

virtual void generateAttitudeReferences() override#

Generate the front camera, stereo camera and drone attitudes references. The camera references do not depend on the drone ones, but will be saturated if not reachable.

virtual void endStep() override#

Finish a step of the guidance Output data shall be valid after this point

virtual void exit() override#

Called when the mode is de-activated.

Public Static Functions

static int init(Guidance *guidance)#
static void destroy()#

Protected Functions

bool updateResetStatus()#

Update the timer for checking if a mode should be reset.

This function should be called in each TrajectoryFcamMode mode

  • in the configure() function to decide wether the mode should be reset

  • in the beginStep() function to update the timer

Returns:

true if the mode should be reset

void updateTelemetry()#

Update variables from latest telemetry.

Convert estimated position trajectory point from global to local. Convert estimated drone Euler angles from NED start to NED.

Protected Attributes

bool mFcamGroundConstraintEnabled#
float mFcamYawAccelerationNedStartRef#
float mFcamZoom#
::ColibryLite::Messages::HorizontalControlConfig::Enum mDroneHorizontalControlConfig#
::ColibryLite::Messages::VerticalControlConfig::Enum mDroneVerticalControlConfig#
::ColibryLite::Messages::YawControlConfig::Enum mDroneYawControlConfig#

Protected Static Attributes

static timespec sLastCallTimestamp#
static std::unique_ptr<boonta::IElcanoMpc> sReplannerLocal#
static unsigned int sReplannerReferenceStatus#
static bool sIsReplannerStuck = false#
static bool sHasReplannerFailed = false#
static Eigen::Vector3f sClosestObstaclePositionLocal#
static float sClosestObstacleSignedDistance#
static bool sIsInitialized = false#
static guidance::PositionTrajectoryPoint sDronePositionStateLocalEst#
static guidance::PositionTrajectoryPoint sDronePositionStateLocalNom#
static guidance::PositionTrajectoryPoint sDronePositionStateLocalRef#
static guidance::EulerAnglesZyx sDroneEulerAnglesNedEst#
static guidance::EulerAnglesZyx sDroneEulerAnglesNedStartEst#
static Eigen::Vector3f sDroneYawStateNedStartRef#
static float sDroneMaxHorizSpeed#
static float sDroneMinVerticalSpeed#
static float sDroneMaxVerticalSpeed#
static guidance::EulerAnglesZyx sStcamEulerAnglesNedStartRef#
class TrajectoryStcamMode : public guidance::Mode#

Public Functions

TrajectoryStcamMode(Guidance *guidance, const std::string &name)#
virtual ~TrajectoryStcamMode() override#
void setDroneNominalPositionTrajectoryGlobal(const guidance::PositionTrajectoryPoint &point)#

Set nominal global drone trajectory (to be called from generateDroneReference)

void setDroneNominalPositionTrajectoryLocal(const guidance::PositionTrajectoryPoint &point)#

Set nominal local drone trajectory (to be called from generateDroneReference)

void setFcamEulerAnglesZyxNed(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setFcamEulerAnglesZyxNedStart(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set front camera angles and derivative angles (to be called from generateFcamAttitudeReference) in NED Start

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setStcamEulerAnglesZyxNed(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set stereo camera angles and derivative angles (to be called from generateStcamAttitudeReference) in NED

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

void setStcamEulerAnglesZyxNedStart(const guidance::EulerAnglesZyx &angles, const guidance::EulerAnglesZyx &derivatives)#

Set stereo camera angles and derivative angles (to be called from generateStcamAttitudeReference) in NED Start

Parameters:
  • angles – are euler angles

  • derivatives – are 1st derivatives of euler angles

float getMaxHorizontalSpeed() const#

Return the maximum admissible horizontal speed [m/s] (> 0)

float getMaxDescentSpeed() const#

Return the maximum admissible descent speed [m/s] (> 0)

float getMaxAscentSpeed() const#

Return the maximum admissible ascent speed [m/s] (> 0)

virtual void generateFcamAttitudeReference() = 0#

Called at the beginning of generateAttitudeReferences to let derived class to fill mFcamEulerAnglesNedStartRef, mFcamEulerAnglesDerivativesNedStartRef, mFcamAcceleration and mFcamZoom.

virtual void generateStcamAttitudeReference() = 0#

Called at the beginning of generateAttitudeReferences to let derived class to fill mStcamEulerAnglesNedStartRef, mStcamEulerAnglesDerivativesNedStartRef and mStcamAcceleration.

inline uint32_t getTickPeriod() const#
inline float getTimePeriod() const#
inline const PositionTrajectoryPoint &getDroneNominalPositionTrajectoryLocal() const#
inline const PositionTrajectoryPoint &getDroneReferencePositionTrajectoryLocal() const#
virtual bool hasObstacleAvoidance() override#

Return whether the mode implements obstacle avoidance Unless this function is overriden, modes do not advertise they implement obstacle avoidance. TODO: Add to mode descriptor

virtual void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period) override#

Get the triggers for the mode

Parameters:
  • triggers – mask of Trigger values. TRIGGER_TICK can not be combined with TRIGGER_TIMER.

  • timeout – if no frames received during this time (in ms), force a step.

  • period – if a timer is required, its period (in ms). If there is also a trigger on frame with a timeout specified, it will only be called when there is no frame. For tick trigger, this is the period of wakeup measured in ticks of the Drone Controller. For a 200Hz tick in drone controller this means a increment of 5ms per tick.

virtual void configure(const ::google::protobuf::Any &config, bool disableObstacleAvoidance, bool overrideFrontCamera, bool overrideStereoCamera) override#

Called when the mode is set with a configuration

virtual void beginStep() override#

Start a new step of the guidance The input data is valid at this point

virtual void correctDroneReference() override#

Correct the generated trajectory using obstacle avoidance data.

virtual void generateAttitudeReferences() override#

Generate the front camera, stereo camera and drone attitudes references. The camera references do not depend on the drone ones, but will be saturated if not reachable.

virtual void endStep() override#

Finish a step of the guidance Output data shall be valid after this point

Protected Attributes

bool mStcamGroundConstraintEnabled#
float mStcamYawAccelerationNedStartRef#
float mFcamZoom#

Native specific#

It is possible to declare from one to several modes in a .cpp file. The files where modes are implemented must contain two functions that allow to use them:

static FirstMode *sFirstMode;
static SecondMode *sSecondMode;

void guidance_plugin_init(guidance::Guidance *guidance)
{
    sFirstMode = new FirstMode(guidance);
    sSecondMode = new SecondMode(guidance);
    guidance->addMode(sFirstMode);
    guidance->addMode(sSecondMode);
}

void guidance_plugin_shutdown(guidance::Guidance *guidance)
{
    guidance->removeMode(sFirstMode);
    delete sFirstMode;
    sFirstMode = nullptr;
    guidance->removeMode(sSecondMode);
    delete sSecondMode;
    sSecondMode = nullptr;
}

Python specific#

It is possible to declare from one to several modes in a .py file. The files where modes are implemented must contain a dictionary that allows to use them.

GUIDANCE_MODES = {
    'first_mode': FirstModeClass,
    'second_mode': SecondModeClass
}

All methods from guidance::Mode class use snake case instead of camel case, except for the guidance::Mode::getName() function which does not exist in Python.

Example:

Native method

void getTriggers(uint32_t *triggers, uint32_t *timeout, uint32_t *period)
{
    *triggers = guidance::TRIGGER_TICK;
    *timeout = 0;
    *period = 6;
}

becomes

def get_triggers(self):
    return (gdnc_core.Trigger.TICK, 0, 6)