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

OP-1760 autotakeoff

Browse Source 
1. Make autotakeoff height configurable
2. Fix the pidcontroldown transfer to solve the rough transition between pid controllers for landing, takeoff, braking etc.
This commit is contained in:
abeck70 2015-04-16 21:22:37 +10:00
parent a8c6a19784
commit 04c42bd316
4 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/libraries/plans.c
View File

@ -179,15 +179,25 @@ void plan_setup_returnToBase()
static StatusVtolAutoTakeoffControlStateOptions autotakeoffState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT 2.5f
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
static void plan_setup_AutoTakeoff_helper(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
float autotakeoff_height;
FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
autotakeoff_height = fabsf(autotakeoff_height);
if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
} else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
}
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
@ -199,7 +209,7 @@ static void plan_setup_AutoTakeoff_helper(PathDesiredData *pathDesired)
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down - AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT;
pathDesired->End.Down = positionState.Down - autotakeoff_height;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;

3
flight/modules/PathFollower/pidcontroldown.cpp
View File

@ -89,8 +89,7 @@ void PIDControlDown::Activate()
float currentThrust;
StabilizationDesiredThrustGet(&currentThrust);
float u0 = currentThrust - mNeutral;
pid2_transfer(&PID, u0);
pid2_transfer(&PID, currentThrust);
mActive = true;
}

5
flight/modules/PathFollower/pidcontrolne.cpp
View File

@ -61,11 +61,6 @@ void PIDControlNE::Deactivate()
void PIDControlNE::Activate()
{
// Do we need to initialise any loops for smooth transition
// float currentNE;
// StabilizationDesiredNEGet(&currentNE);
// float u0 = currentNE - mNeutral;
// pid2_transfer(&PID, u0);
mActive = true;
}

1
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -113,6 +113,7 @@
<field name="ReturnToBaseNextCommand" units="" type="enum" elements="1" options="Hold,Land" defaultvalue="Hold"/>
<field name="LandingVelocity" units="m" type="float" elements="1" defaultvalue="0.6"/>
<field name="AutoTakeOffVelocity" units="m" type="float" elements="1" defaultvalue="0.6"/>
<field name="AutoTakeOffHeight" units="m" type="float" elements="1" defaultvalue="2.5" description="height in meters above arming altitude to climb to during autotakeoff"/>
<field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="30,15" description="stick sensitivity for position roam modes"/>
<field name="VarioControlLowPassAlpha" units="" type="float" elements="1" defaultvalue="0.98" description="stick low pass filter for position roam modes"/>
<access gcs="readwrite" flight="readwrite"/>