rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

OP-1350 Make RTB safer pick the lowest between PositionState.Down and TakeOffLocation.Down.

Browse Source
This commit is contained in:
Alessio Morale 2014-05-18 14:41:45 +02:00
parent 2048440053
commit d88d73e7fe
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/modules/ManualControl/pathfollowerhandler.c
View File

@ -34,7 +34,7 @@
#include <flightstatus.h> #include <flightstatus.h>
#include <positionstate.h> #include <positionstate.h>
#include <flightmodesettings.h> #include <flightmodesettings.h>
#include <pios_math.h>
#if defined(REVOLUTION) #if defined(REVOLUTION)
#include <takeofflocation.h> #include <takeofflocation.h>
@ -71,20 +71,26 @@ void pathFollowerHandler(bool newinit)
TakeOffLocationGet(&takeoffLocation); TakeOffLocationGet(&takeoffLocation);
switch (flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
{
// Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
// TODO: right now VTOLPF does fly straight to destination altitude.
// For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
float destDown = MIN(positionState.Down, takeoffLocation.Down) - settings.ReturnToBaseAltitudeOffset;
pathDesired.Start.North = takeoffLocation.North; pathDesired.Start.North = takeoffLocation.North;
pathDesired.Start.East = takeoffLocation.East; pathDesired.Start.East = takeoffLocation.East;
pathDesired.Start.Down = positionState.Down - settings.ReturnToBaseAltitudeOffset; pathDesired.Start.Down = destDown;
pathDesired.End.North = takeoffLocation.North; pathDesired.End.North = takeoffLocation.North;
pathDesired.End.East = takeoffLocation.East; pathDesired.End.East = takeoffLocation.East;
pathDesired.End.Down = positionState.Down - settings.ReturnToBaseAltitudeOffset; pathDesired.End.Down = destDown;
pathDesired.StartingVelocity = 1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity = 0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
break; }
break;
default: default:
pathDesired.Start.North = positionState.North; pathDesired.Start.North = positionState.North;