rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1156 Made PathPlanner work with delayed callbacks in "navigation" callback task

Browse Source 
Conflicts:
	flight/modules/PathPlanner/pathplanner.c
This commit is contained in:
Corvus Corax 2014-01-11 20:03:36 +01:00
parent 2ce0cb7909
commit b8118f51e7
1 changed files with 116 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
flight/modules/PathPlanner/pathplanner.c
View File

@ -47,15 +47,17 @@
// Private constants
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define TASK_PRIORITY CALLBACK_TASK_NAVIGATION
#define MAX_QUEUE_SIZE 2
#define PATH_PLANNER_UPDATE_RATE_MS 20
#define PATH_PLANNER_UPDATE_RATE_MS 100 // can be slow, since we listen to status updates as well
// Private types
// Private functions
static void pathPlannerTask(void *parameters);
static void updatePathDesired(UAVObjEvent *ev);
static void pathPlannerTask();
static void commandUpdated(UAVObjEvent *ev);
static void statusUpdated(UAVObjEvent *ev);
static void updatePathDesired();
static void setWaypoint(uint16_t num);
static uint8_t pathConditionCheck();
@ -72,7 +74,8 @@ static uint8_t conditionImmediate();
// Private variables
static xTaskHandle taskHandle;
static DelayedCallbackInfo *pathPlannerHandle;
static DelayedCallbackInfo *pathDesiredUpdaterHandle;
static WaypointActiveData waypointActive;
static WaypointData waypoint;
static PathActionData pathAction;
@ -84,11 +87,14 @@ static bool pathplanner_active = false;
*/
int32_t PathPlannerStart()
{
taskHandle = NULL;
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(commandUpdated);
WaypointActiveConnectCallback(commandUpdated);
PathActionConnectCallback(commandUpdated);
PathStatusConnectCallback(statusUpdated);
// Start VM thread
xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
// Start main task callback
DelayedCallbackDispatch(pathPlannerHandle);
return 0;
}
@ -98,8 +104,6 @@ int32_t PathPlannerStart()
*/
int32_t PathPlannerInitialize()
{
taskHandle = NULL;
FlightPlanInitialize();
PathActionInitialize();
PathStatusInitialize();
@ -110,6 +114,9 @@ int32_t PathPlannerInitialize()
WaypointInitialize();
WaypointActiveInitialize();
pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES);
return 0;
}
@ -118,26 +125,18 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
/**
* Module task
*/
static void pathPlannerTask(__attribute__((unused)) void *parameters)
static void pathPlannerTask()
{
// when the active waypoint changes, update pathDesired
WaypointConnectCallback(updatePathDesired);
WaypointActiveConnectCallback(updatePathDesired);
PathActionConnectCallback(updatePathDesired);
DelayedCallbackSchedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
bool endCondition = false;
FlightStatusData flightStatus;
PathDesiredData pathDesired;
PathStatusData pathStatus;
// Main thread loop
bool endCondition = false;
while (1) {
vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS);
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false;
continue;
return;
}
WaypointActiveGet(&waypointActive);
@ -149,28 +148,29 @@ static void pathPlannerTask(__attribute__((unused)) void *parameters)
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
continue;
return;
}
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
PathStatusData pathStatus;
PathStatusGet(&pathStatus);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
// delay next step until path follower has acknowledged the path mode
if (pathStatus.UID != pathDesired.UID) {
continue;
return;
}
// negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
continue;
return;
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
@ -205,42 +205,49 @@ static void pathPlannerTask(__attribute__((unused)) void *parameters)
}
break;
}
}
}
// callback function when status changed, issue execution of state machine
void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathDesiredUpdaterHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
{
DelayedCallbackDispatch(pathPlannerHandle);
}
// callback function when waypoints changed in any way, update pathDesired
void updatePathDesired()
{
// only ever touch pathDesired if pathplanner is enabled
if (!pathplanner_active) {
return;
}
// use local variables, dont use stack since this is huge and a callback,
// dont use the globals because we cant use mutexes here
static WaypointActiveData waypointActiveData;
static PathActionData pathActionData;
static WaypointData waypointData;
static PathDesiredData pathDesired;
PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActiveData);
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActiveData.Index, &waypointData);
PathActionInstGet(waypointData.Action, &pathActionData);
WaypointInstGet(waypointActive.Index, &waypoint);
PathActionInstGet(waypoint.Action, &pathAction);
pathDesired.End.North = waypointData.Position.North;
pathDesired.End.East = waypointData.Position.East;
pathDesired.End.Down = waypointData.Position.Down;
pathDesired.EndingVelocity = waypointData.Velocity;
pathDesired.Mode = pathActionData.Mode;
pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1];
pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2];
pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3];
pathDesired.UID = waypointActiveData.Index;
pathDesired.End.North = waypoint.Position.North;
pathDesired.End.East = waypoint.Position.East;
pathDesired.End.Down = waypoint.Position.Down;
pathDesired.EndingVelocity = waypoint.Velocity;
pathDesired.Mode = pathAction.Mode;
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
pathDesired.UID = waypointActive.Index;
if (waypointActiveData.Index == 0) {
if (waypointActive.Index == 0) {
PositionStateData positionState;
PositionStateGet(&positionState);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)