/**
* Increment the waypoint index which triggers the active waypoint method
*/
static void advanceWaypoint()
{
WaypointActiveGet(&waypointActive);
// Store the currently active waypoint. This is used in activeWaypoint to plot
// a waypoint from this (previous) waypoint to the newly selected one
previous_waypoint = waypointActive.Index;
// Default implementation simply jumps to the next possible waypoint. Insert any
// conditional logic desired here.
// Note: In the case of conditional logic it is the responsibilty of the implementer
// to ensure all possible paths are valid.
waypointActive.Index++;
if (waypointActive.Index >= UAVObjGetNumInstances(WaypointHandle())) {
holdCurrentPosition();
// Do not reset path_status_updated here to avoid this method constantly being called
return;
} else {
WaypointActiveSet(&waypointActive);
}
// Invalidate any pending path status updates
path_status_updated = false;
}
void WaypointModeGet( uint8_t *NewMode )
{
UAVObjGetDataField(WaypointHandle(), (void*)NewMode, offsetof( WaypointData, Mode), sizeof(uint8_t));
}
void WaypointModeParametersGet( float *NewModeParameters )
{
UAVObjGetDataField(WaypointHandle(), (void*)NewModeParameters, offsetof( WaypointData, ModeParameters), sizeof(float));
}
void WaypointVelocityGet( float *NewVelocity )
{
UAVObjGetDataField(WaypointHandle(), (void*)NewVelocity, offsetof( WaypointData, Velocity), sizeof(float));
}
void WaypointPositionGet( float *NewPosition )
{
UAVObjGetDataField(WaypointHandle(), (void*)NewPosition, offsetof( WaypointData, Position), 3*sizeof(float));
}
/**
* This method is called when a new waypoint is activated
*
* Note: The way this is called, it runs in an object callback. This is safe because
* the execution time is extremely short. If it starts to take a longer time then
* the main task look should monitor a flag (such as the waypoint changing) and call
* this method from the main task.
*/
static void activateWaypoint(int idx)
{
active_waypoint = idx;
if (idx >= UAVObjGetNumInstances(WaypointHandle())) {
// Attempting to access invalid waypoint. Fall back to position hold at current location
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLANNER, SYSTEMALARMS_ALARM_ERROR);
holdCurrentPosition();
return;
}
// Get the activated waypoint
WaypointInstGet(idx, &waypoint);
PathDesiredData pathDesired;
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.ModeParameters = waypoint.ModeParameters;
// Use this to ensure the cases match up (catastrophic if not) and to cover any cases
// that don't make sense to come from the path planner
switch(waypoint.Mode) {
case WAYPOINT_MODE_FLYVECTOR:
pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR;
break;
case WAYPOINT_MODE_FLYENDPOINT:
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
break;
case WAYPOINT_MODE_FLYCIRCLELEFT:
pathDesired.Mode = PATHDESIRED_MODE_FLYCIRCLELEFT;
break;
case WAYPOINT_MODE_FLYCIRCLERIGHT:
pathDesired.Mode = PATHDESIRED_MODE_FLYCIRCLERIGHT;
break;
case WAYPOINT_MODE_LAND:
pathDesired.Mode = PATHDESIRED_MODE_LAND;
break;
default:
holdCurrentPosition();
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLANNER, SYSTEMALARMS_ALARM_ERROR);
return;
}
pathDesired.EndingVelocity = waypoint.Velocity;
if(previous_waypoint < 0) {
// For first waypoint, get current position as start point
PositionActualData positionActual;
PositionActualGet(&positionActual);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 1;
pathDesired.StartingVelocity = waypoint.Velocity;
} else {
// Get previous waypoint as start point
WaypointData waypointPrev;
WaypointInstGet(previous_waypoint, &waypointPrev);
pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
pathDesired.StartingVelocity = waypointPrev.Velocity;
}
PathDesiredSet(&pathDesired);
// Invalidate any pending path status updates
path_status_updated = false;
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLANNER);
}