rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

OP-1760 autotakeoff in pathplaner: control to start location instead of end location in NE

Browse Source
This commit is contained in:
abeck70 2015-04-17 22:44:05 +10:00
parent 55cbc2cd9b
commit 88b0c99087
2 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
View File

@ -117,8 +117,15 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
// often be waypoint 0, in which case the starting location is the current location, and the end location
// is the waypoint location which really needs to be the same as the end in north and east. If it is not,
// it will fly to that location the during ascent.
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
// Note in pathplanner mode we use the start location which is the current initial location as the
// target NE to control. Takeoff only ascends vertically.
controlNE.UpdatePositionSetpoint(pathDesired->Start.North, pathDesired->Start.East);
// Sanity check that the end location is at least a reasonable height above the start location in the down direction
float autotakeoff_height = pathDesired->Start.Down - pathDesired->End.Down;
if (autotakeoff_height < 2.0f) {
pathDesired->End.Down = pathDesired->Start.Down - 2.0f;
}
controlDown.UpdatePositionSetpoint(pathDesired->End.Down); // the altitude is set by the end location.
fsm->setControlState(STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE);
}
}
@ -194,7 +201,6 @@ void VtolAutoTakeoffController::UpdateVelocityDesired()
controlDown.UpdateVelocityState(velocityState.Down);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// autotakeoff flight mode has stored original horizontal position in pathdesired
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();

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

@ -403,6 +403,8 @@ void updatePathDesired()
/*pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];*/
// note takeoff relies on the start being the current location as it merely ascends and using
// the start as assumption current NE location
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;