ICAROUS
All Classes Namespaces Files Functions Variables Enumerations Enumerator Macros Modules Pages
Public Member Functions | Private Member Functions | Private Attributes | List of all members
Guidance Class Reference

Core class for performing guidance computations.

#include <Guidance.hpp>

Public Member Functions

 Guidance (const std::string config)
 Construct a new Guidance object. More...
 
void ReadParamFromFile (std::string config)
 Read parameters from file. More...
 
void SetGuidanceParams (const GuidanceParams_t *params)
 Set the Guidance Params object. More...
 
void SetAircraftState (const larcfm::Position &pos, const larcfm::Velocity &groundSpeed)
 Set the Aircraft State. More...
 
void SetWindData (const double windFrom, const double windSpeed)
 Set the Wind Data. More...
 
void InputFlightplanData (const std::string &plan_id, const std::list< waypoint_t > &waypoints, const double initHeading, bool repair, double repairTurnRate)
 Input flightplan. More...
 
int RunGuidance (double time)
 Perform guidance computations for given time. More...
 
void SetGuidanceMode (const GuidanceMode mode, const std::string planID, const int nextWP, const bool eta)
 Set the Guidance Mode. More...
 
void ChangeWaypointAlt (const std::string planID, int wpID, const double value, const bool updateAll)
 Change altitude in flightplan segment. More...
 
void ChangeWaypointSpeed (const std::string planID, int wpID, const double value)
 Change speed in flightplan segement. More...
 
void ChangeWaypointETA (const std::string planID, const int wpID, const double value, const bool updateAll)
 Change eta constraints in flightplan. More...
 
void SetVelocityCommands (const larcfm::Velocity &inputs)
 Set the Velocity Commands (used when an external modules is computing guidance commands) More...
 
void GetOutput (GuidanceOutput_t &output)
 Get the Output object. More...
 
int GetWaypoint (const std::string planID, int id, waypoint_t &wp)
 Get Waypoint. More...
 

Private Member Functions

double ComputeOffSetPositionOnPlan (larcfm::Plan *fp, int nextWP, double guidance_radius, larcfm::Position &pos)
 Function to compute the intersection of the line of sight circle with current plan segment. More...
 
larcfm::Vect3 GetCorrectIntersectionPoint (const larcfm::Vect3 &wpA, const larcfm::Vect3 &wpB, const double r)
 Returns the intersection point between a circle of radius r and the line between wpA and wpB. current position is the origin and wpA and wpB are defined with respect to origin. More...
 
double distance (double x1, double y1, double x2, double y2)
 Compute distance given two points. More...
 
double ComputeSpeed ()
 Compute current speed that should be used for output command. More...
 
double ComputeClimbRate (double speedRef)
 Compute current climb rate for the output. More...
 
double ComputeNewHeading (double &speedRef)
 Compute new heading. More...
 
double GetApproachPrecision (const larcfm::Position &position, const larcfm::Velocity &velocity, const larcfm::Position &waypoint)
 A dot product computation to check if vehicle has crossed the waypoint. < 0 if crossed. >= 0 if not crossed. More...
 
larcfm::PlanGetPlan (const std::string &plan_id)
 Get the Plan object. More...
 
void FilterCommand (double &refHeading, double &refSpeed, double &refVS)
 Filter command (rate limits and saturations) More...
 
void ComputePlanGuidance ()
 Perform guidance computation. More...
 
void CheckWaypointArrival ()
 Check waypoint arrival. More...
 

Private Attributes

std::list< larcfm::PlanplanList
 List of all plans received by guidance.
 
std::map< std::string, int > nextWpId
 Map from flight plan id to next waypoint id.
 
std::string activePlanId
 Plan ID for active plan.
 
std::string prevPlan
 Plan ID for previous plan.
 
larcfm::PlancurrentPlan
 Pointer to current plan data structure.
 
larcfm::Position currentPos
 Current position.
 
larcfm::Velocity currentAirspeed
 Current velocity (airspeed)
 
larcfm::Velocity currentGroundSpeed
 Current velocity (ground speed)
 
larcfm::Velocity outputCmd
 Output command computed by guidance.
 
GuidanceMode mode
 Current guidance mode.
 
double currTime
 Current time.
 
bool wpReached
 Flag indicating waypoint arrival.
 
bool etaControl
 Flag indicating eta control.
 
bool inTurn
 Flag indicating aircraft is in a turn.
 
double distH2nextWP
 Horizontal distance to next waypoint.
 
double distV2nextWP
 Vertical distance to next waypoint.
 
double xtrackDist
 Cross track deviation.
 
double prevTrackControllerTime
 Time at which last cycle of the controller was run.
 
double prevTrackControllerTarget
 Previous target command for the controller.
 
larcfm::Velocity wind
 Wind velocity (to)
 
GuidanceParams_t params
 Guidance parameters.
 

Constructor & Destructor Documentation

◆ Guidance()

Guidance::Guidance ( const std::string  config)
Parameters
configconfiguration filename

Member Function Documentation

◆ ChangeWaypointAlt()

void Guidance::ChangeWaypointAlt ( const std::string  planID,
int  wpID,
const double  value,
const bool  updateAll 
)
Parameters
planIDid of plan
wpIDindex of waypoint for which altitude must be changed
valuevalue of new altitude
updateAllupdate altitude for entire plan from wpID

For altitude changes, create a copy of the original plan and modify altitudes in the copy.

New Plan

Reuse existing PlanAltChange if available

Find diff altitude so that all remaining waypoints can be shifted by the diff altitude

If no altitude difference, revert back to using nominal plan

Shift altitudes in plan copy

Add new plan to list if not already available

Set pointers to the new plan

◆ ChangeWaypointETA()

void Guidance::ChangeWaypointETA ( const std::string  planID,
const int  wpID,
const double  value,
const bool  updateAll 
)
Parameters
planIDID of plan
wpIDindex of waypoint at which eta should change
valuevalue of new eta
updateAllupdate eta for rest of flightplan

◆ ChangeWaypointSpeed()

void Guidance::ChangeWaypointSpeed ( const std::string  planID,
int  wpID,
const double  value 
)
Parameters
planIDif of plan
wpIDindex of waypoint for which speed must be changed
valuevalue of new speed

Request a speed change on flightplan leg between wpid-1 and wpid.

  • If wpid is -1, then use the next waypoint index (the waypoint we are currently flying towards)

Speed change is between leg wpidprev and newInd

If the speed change is the same as the speed defined in the original flightplan, no new flight plan is required.

Create a new copy of nominal plan so that we can change change the speed in this new flightplan

Shift the time based on the leg length and new speed
Note, we only shift from the wpid that is requested.

Check to see if there was a previous plan for speed change

Use this plan if no previous plans.

Clear previous speed change plan and use new speed change plan.

◆ CheckWaypointArrival()

void Guidance::CheckWaypointArrival ( )
private
  • Get horizontal distance to next waypoint
  • Get vertical distance to next waypoint
  • Scale capture radius based on current speed of vehicle
  • Impose user defined limits on capture radius
  • Compute approach precision value
  • Aritificially constrain precision approach to -1 for linear plans. i.e Don't use precision checks for linear plans
  • If distance to next waypoint is < captureRadius, switch to next waypoint

◆ ComputeClimbRate()

double Guidance::ComputeClimbRate ( double  speedRef)
private
Parameters
speedRefcomputed speed
Returns
double output climb rate

Check if this is a climb segment

Over large altitude changes outside the climbAngleVRange params, use bangbang control

Use extremal values for climb rate

For eta, use the required climb rate based on arrival constraint

Not eta, pull climb rate from flightplan

In linear case, calculate climb rate based on flightpath angle

Climb rate determined by the trig relation: speedRef is adjacent, climbrate is opposite

In kinematic case, use prescribed climb rate

Use simple proportional control scheme when not in bangbang mode

Impose saturation limits

◆ ComputeNewHeading()

double Guidance::ComputeNewHeading ( double &  speedRef)
private
Parameters
speedRefcomputed speed
Returns
double output heading

Check to determine if the aircraft is currently in a turn segment If prev waypoint is a Track TCP (i.e. BOT or MOT), compute new heading based on heading delta in segment

  • Get prev track tcp waypoint index
  • Get center of turn for track tcp
  • Heading of vector from center to prev waypoint (NOTE: prev waypoint could be BOT or MOT because we are current in a turn)
  • Heading of vector from center to next waypoint
  • Heading of vector from center to current position
  • Output heading once the vehicle finishes the turn. i.e. Heading at EOT
  • Radius of turn
  • Turn direction
  • The ideal heading the vehicle should be having at it's current position
  • The actual heading of the vehicle
  • Total heading change from trk1 to trk2
  • Heading change from trk1 to ideal heading at current position
  • Assume that that angle between BOT and EOT is no more than 180 degrees

ETA control in a turn

  • Compute turn rate from flightplan
  • Compute remaining time in turn according to plan
  • Compute actual remaining time
  • Increase turn rate if actual time remaining is less than planned time remaining
  • Decrease turn rate if actual time remaining is more than planned time remaining
  • Compute reference speed from turn rate and radius (v = r x \omega)

Compute offset from center. This is 0 if vehicle is exactly on the turn, < 0 if inside the turn and > 0 if outside the turn

Compute heading change based on offset

Compute new heading

If prev waypoint is not Track TCP or is EOT, use the line of sight circle method to compute heading

◆ ComputeOffSetPositionOnPlan()

double Guidance::ComputeOffSetPositionOnPlan ( larcfm::Plan fp,
int  nextWP,
double  guidance_radius,
larcfm::Position pos 
)
private
Parameters
fpPointer to flightplan
nextWPnext waypoint index (waypoint index the aircraft is currently flying to)
guidance_radiusradius of the line of sight circle
pos[out] offset position on plan
Returns
double cross track deviation from nominal path

Treat the previous waypoint as the origin

Create vectors for wpA->wpB and wpA->ownship_position

Get distance of flightplan segment (AB)

Get distance from waypoint to current position (AP)

Get distance from current position to next waypoint (PB)

Compute projection of AP onto AB

Compute closest point to P on flightplan

Get perpendicular deviation from projection and AP

Convert waypoints to local frame (with ownship position as origin)

  • If within guidance radius from wpB, track to wpB
  • If within guidance radius from wpA, use guidance circle method

Otherwise, check projection of position onto flight plan

If behind wpA, track to wpA

If past wpB, track to wpB

If between wpA and wpB

  • If close enough to flight plan, use guidance circle method
  • If far from flight plan, track to closest point on flight plan

◆ ComputePlanGuidance()

void Guidance::ComputePlanGuidance ( )
private

Get next waypoint index

Return with 0 velocity if the flightplan was completed

Compute to new speed to track

Compute new heading to track

Compute climb rate to track

Filter output commands

Construct output velocity object

Check if the waypoint was reached

◆ ComputeSpeed()

double Guidance::ComputeSpeed ( )
private
Returns
double

Don't perform ETA controls during Speed change, Alt change or during ditching

  • Get intended speed stored in the flightplan when no eta control is required
  • If eta control is required, compute speed based on eta constraints

if timediff is negative, we are running late

◆ distance()

double Guidance::distance ( double  x1,
double  y1,
double  x2,
double  y2 
)
private
Parameters
x1x coordinate
y1y coordinate
x2x coordinate
y2y coordinate
Returns
double distance

◆ FilterCommand()

void Guidance::FilterCommand ( double &  refHeading,
double &  refSpeed,
double &  refVS 
)
private
Parameters
refHeadingoutput heading
refSpeedoutput speed
refVSoutput climb rate

Reduce speed if approaching final waypoint or if turning sharply

Low pass filter for speed and vertical speed commands n factor determines when low pass filter is being applied.

◆ GetApproachPrecision()

double Guidance::GetApproachPrecision ( const larcfm::Position position,
const larcfm::Velocity velocity,
const larcfm::Position waypoint 
)
private
Parameters
positioncurrent position
velocitycurrent velocity
waypointwaypoint flying towards
Returns
double dot product value

Look at the dot product of current velocity with the position of the waypoint in relative NED coordinates dot product >= 0 IMPLIES we are approaching the waypoint, dot procuct < 0 IMPLIES we are moving away.

◆ GetCorrectIntersectionPoint()

larcfm::Vect3 Guidance::GetCorrectIntersectionPoint ( const larcfm::Vect3 wpA,
const larcfm::Vect3 wpB,
const double  r 
)
private
Parameters
wpAstarting waypoint of segment
wpBending waypoint of segment
rradius of line of sight circle
Returns
larcfm::Vect3 Intersection of circle with plan segment

Compute intersection of circle of radius r and line joining point A and point B

  • No solution when complex roots

There can be at most 2 intersections. Return intersection which is closest to waypoint B

◆ GetOutput()

void Guidance::GetOutput ( GuidanceOutput_t output)
Parameters
output

If we are doing a speed change or altitude change, we are still on the the original flighttrack (over the ground). So use the previous plan id This is required because the cognition module doesn't know about these internal speedchange or alt change plans and hence won't be able to keep track of waypoint progress if you don't use the old id in the output.

◆ GetPlan()

larcfm::Plan * Guidance::GetPlan ( const std::string &  plan_id)
private
Parameters
plan_idid of plan
Returns
larcfm::Plan* pointer to plan

◆ GetWaypoint()

int Guidance::GetWaypoint ( const std::string  planID,
int  id,
waypoint_t wp 
)
Parameters
planIDid of plan
idwaypoint index
wp[out] waypoint object
Returns
int

◆ InputFlightplanData()

void Guidance::InputFlightplanData ( const std::string &  plan_id,
const std::list< waypoint_t > &  waypoints,
const double  initHeading,
bool  repair,
double  repairTurnRate 
)
Parameters
plan_idflightplan id
waypointslist of waypoints
initHeadinginitial path heading (only needed for repairing)
repairset to true to convert linear plan to kinematic plan using given turn rate for repair
repairTurnRateturn rate to use for repairing

◆ ReadParamFromFile()

void Guidance::ReadParamFromFile ( std::string  config)
Parameters
configconfiguration filename

◆ RunGuidance()

int Guidance::RunGuidance ( double  time)
Parameters
time
Returns
int
  • FLIGHTPLAN and POINT2POINT modes are implemented identically
  • VECTOR mode receives velocity commands from external sources
  • Currently TAKEOFF mode does nothing
  • LAND mode controls the rate of descent
  • SPEED_CHANGE/ALT_CHANGE are implemented by FLIGHTPLAN by changing speed/alt in flight plan
  • Guidance NOOP mode

◆ SetAircraftState()

void Guidance::SetAircraftState ( const larcfm::Position pos,
const larcfm::Velocity groundSpeed 
)
Parameters
poscurrent position
groundSpeedcurrent ground speed

◆ SetGuidanceMode()

void Guidance::SetGuidanceMode ( const GuidanceMode  mode,
const std::string  planID,
const int  nextWP,
const bool  eta 
)
Parameters
modeguidance mode
planIDplan id
nextWPwaypoint to which aircraft should start flying to
etaenable eta constraints

VECTOR/LAND modes don't use any plans.

Check if the requested plan for the mode is available

  • If nextWP is 0, this means we are starting this plan
  • Shift the times based on the current time.
  • Time shifting is necessary if we are going to maintain ETA.
  • Ensure this function is always called with nextWP = 0 if ETA maintenance is required.

Set initial conditions for nextwp

◆ SetGuidanceParams()

void Guidance::SetGuidanceParams ( const GuidanceParams_t params)
Parameters
params

◆ SetVelocityCommands()

void Guidance::SetVelocityCommands ( const larcfm::Velocity inputs)
Parameters
inputsinput velocity commands

◆ SetWindData()

void Guidance::SetWindData ( const double  windFrom,
const double  windSpeed 
)
Parameters
windFromwind from direction
windSpeedwind speed

The documentation for this class was generated from the following files: