rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1867 pathplanner autoland

Browse Source 
Setup land with descent rate set from flight mode settings.
This commit is contained in:
abeck70 2015-04-30 22:54:38 +10:00
parent 13375ff0f5
commit 0eb84cd8f6
1 changed files with 33 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,6 +49,7 @@
#include <sanitycheck.h> #include <sanitycheck.h>
#include <vtolpathfollowersettings.h> #include <vtolpathfollowersettings.h>
#include <statusvtolautotakeoff.h> #include <statusvtolautotakeoff.h>
#include <statusvtolland.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
// Private constants // Private constants
@ -79,6 +80,7 @@ static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript(); static uint8_t conditionPythonScript();
static uint8_t conditionImmediate(); static uint8_t conditionImmediate();
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired);
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired); static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
static void planner_setup_pathdesired(PathDesiredData *pathDesired); static void planner_setup_pathdesired(PathDesiredData *pathDesired);
@ -130,6 +132,8 @@ int32_t PathPlannerInitialize()
VelocityStateInitialize(); VelocityStateInitialize();
WaypointInitialize(); WaypointInitialize();
WaypointActiveInitialize(); WaypointActiveInitialize();
StatusVtolAutoTakeoffInitialize();
StatusVtolLandInitialize();
pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES); pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES); pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
@ -335,6 +339,9 @@ void updatePathDesired()
case PATHACTION_MODE_AUTOTAKEOFF: case PATHACTION_MODE_AUTOTAKEOFF:
planner_setup_pathdesired_takeoff(&pathDesired); planner_setup_pathdesired_takeoff(&pathDesired);
break; break;
case PATHACTION_MODE_LAND:
planner_setup_pathdesired_land(&pathDesired);
break;
default: default:
planner_setup_pathdesired(&pathDesired); planner_setup_pathdesired(&pathDesired);
break; break;
@ -508,6 +515,32 @@ static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
pathDesired->UID = waypointActive.Index; pathDesired->UID = waypointActive.Index;
} }
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
FlightModeSettingsLandingVelocityGet(&velocity_down);
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN] = velocity_down;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_LAND;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
}
// helper function to go to a specific waypoint // helper function to go to a specific waypoint
static void setWaypoint(uint16_t num) static void setWaypoint(uint16_t num)