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-1858 if previous mode was autotakeoff, ignore wp start position and go from current position.

Browse Source
This commit is contained in:
abeck70 2015-05-04 21:14:54 +10:00
parent 17b8ccd6bb
commit 5ca9d979e6
1 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -80,7 +80,7 @@ static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
static void planner_setup_pathdesired(PathDesiredData *pathDesired);
static void planner_setup_pathdesired(PathDesiredData *pathDesired, uint8_t);
// Private variables
@ -323,20 +323,22 @@ void updatePathDesired()
return;
}
PathDesiredData pathDesired;
// find out current waypoint
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
// Capture if current mode is takeoff
uint8_t autotakeoff = (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF);
PathActionInstGet(waypoint.Action, &pathAction);
PathDesiredData pathDesired;
switch (pathAction.Mode) {
case PATHACTION_MODE_AUTOTAKEOFF:
planner_setup_pathdesired_takeoff(&pathDesired);
break;
default:
planner_setup_pathdesired(&pathDesired);
planner_setup_pathdesired(&pathDesired, autotakeoff);
break;
}
@ -429,7 +431,7 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
}
// Standard setup of a pathDesired command from the waypoint path plan
static void planner_setup_pathdesired(PathDesiredData *pathDesired)
static void planner_setup_pathdesired(PathDesiredData *pathDesired, uint8_t overwrite_start_position)
{
pathDesired->End.North = waypoint.Position.North;
pathDesired->End.East = waypoint.Position.East;
@ -443,7 +445,7 @@ static void planner_setup_pathdesired(PathDesiredData *pathDesired)
pathDesired->UID = waypointActive.Index;
if (waypointActive.Index == 0) {
if (waypointActive.Index == 0 || overwrite_start_position) {
PositionStateData positionState;
PositionStateGet(&positionState);
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)