ICAROUS
|
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::Plan * | GetPlan (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::Plan > | planList |
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::Plan * | currentPlan |
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. | |
Guidance::Guidance | ( | const std::string | config | ) |
config | configuration filename |
void Guidance::ChangeWaypointAlt | ( | const std::string | planID, |
int | wpID, | ||
const double | value, | ||
const bool | updateAll | ||
) |
planID | id of plan |
wpID | index of waypoint for which altitude must be changed |
value | value of new altitude |
updateAll | update 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
void Guidance::ChangeWaypointETA | ( | const std::string | planID, |
const int | wpID, | ||
const double | value, | ||
const bool | updateAll | ||
) |
planID | ID of plan |
wpID | index of waypoint at which eta should change |
value | value of new eta |
updateAll | update eta for rest of flightplan |
void Guidance::ChangeWaypointSpeed | ( | const std::string | planID, |
int | wpID, | ||
const double | value | ||
) |
planID | if of plan |
wpID | index of waypoint for which speed must be changed |
value | value of new speed |
Request a speed change on flightplan leg between wpid-1 and wpid.
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.
|
private |
|
private |
speedRef | computed speed |
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
|
private |
speedRef | computed speed |
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
ETA control in a turn
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
|
private |
fp | Pointer to flightplan |
nextWP | next waypoint index (waypoint index the aircraft is currently flying to) |
guidance_radius | radius of the line of sight circle |
pos | [out] offset position on plan |
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)
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
|
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
|
private |
Don't perform ETA controls during Speed change, Alt change or during ditching
if timediff is negative, we are running late
|
private |
x1 | x coordinate |
y1 | y coordinate |
x2 | x coordinate |
y2 | y coordinate |
|
private |
refHeading | output heading |
refSpeed | output speed |
refVS | output 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.
|
private |
position | current position |
velocity | current velocity |
waypoint | waypoint flying towards |
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.
|
private |
wpA | starting waypoint of segment |
wpB | ending waypoint of segment |
r | radius of line of sight circle |
Compute intersection of circle of radius r and line joining point A and point B
There can be at most 2 intersections. Return intersection which is closest to waypoint B
void Guidance::GetOutput | ( | GuidanceOutput_t & | output | ) |
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.
|
private |
plan_id | id of plan |
int Guidance::GetWaypoint | ( | const std::string | planID, |
int | id, | ||
waypoint_t & | wp | ||
) |
planID | id of plan |
id | waypoint index |
wp | [out] waypoint object |
void Guidance::InputFlightplanData | ( | const std::string & | plan_id, |
const std::list< waypoint_t > & | waypoints, | ||
const double | initHeading, | ||
bool | repair, | ||
double | repairTurnRate | ||
) |
plan_id | flightplan id |
waypoints | list of waypoints |
initHeading | initial path heading (only needed for repairing) |
repair | set to true to convert linear plan to kinematic plan using given turn rate for repair |
repairTurnRate | turn rate to use for repairing |
void Guidance::ReadParamFromFile | ( | std::string | config | ) |
config | configuration filename |
int Guidance::RunGuidance | ( | double | time | ) |
time |
void Guidance::SetAircraftState | ( | const larcfm::Position & | pos, |
const larcfm::Velocity & | groundSpeed | ||
) |
pos | current position |
groundSpeed | current ground speed |
void Guidance::SetGuidanceMode | ( | const GuidanceMode | mode, |
const std::string | planID, | ||
const int | nextWP, | ||
const bool | eta | ||
) |
mode | guidance mode |
planID | plan id |
nextWP | waypoint to which aircraft should start flying to |
eta | enable eta constraints |
VECTOR/LAND modes don't use any plans.
Check if the requested plan for the mode is available
Set initial conditions for nextwp
void Guidance::SetGuidanceParams | ( | const GuidanceParams_t * | params | ) |
params |
void Guidance::SetVelocityCommands | ( | const larcfm::Velocity & | inputs | ) |
inputs | input velocity commands |
void Guidance::SetWindData | ( | const double | windFrom, |
const double | windSpeed | ||
) |
windFrom | wind from direction |
windSpeed | wind speed |