From 04c42bd316e33d477387a62a90cb8b54abb15b25 Mon Sep 17 00:00:00 2001
From: abeck70 <abeck70@icloud.com>
Date: Thu, 16 Apr 2015 21:22:37 +1000
Subject: [PATCH] OP-1760 autotakeoff

1. Make autotakeoff height configurable
2. Fix the pidcontroldown transfer to solve the rough transition between pid controllers for landing, takeoff, braking etc.
---
 flight/libraries/plans.c                          | 14 ++++++++++++--
 flight/modules/PathFollower/pidcontroldown.cpp    |  3 +--
 flight/modules/PathFollower/pidcontrolne.cpp      |  5 -----
 shared/uavobjectdefinition/flightmodesettings.xml |  1 +
 4 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c
index 8e88ae4f2..97a3247f2 100644
--- a/flight/libraries/plans.c
+++ b/flight/libraries/plans.c
@@ -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;
diff --git a/flight/modules/PathFollower/pidcontroldown.cpp b/flight/modules/PathFollower/pidcontroldown.cpp
index 568db3d72..ea01b7e06 100644
--- a/flight/modules/PathFollower/pidcontroldown.cpp
+++ b/flight/modules/PathFollower/pidcontroldown.cpp
@@ -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;
 }
 
diff --git a/flight/modules/PathFollower/pidcontrolne.cpp b/flight/modules/PathFollower/pidcontrolne.cpp
index 6cb871bea..f6ac0f6cd 100644
--- a/flight/modules/PathFollower/pidcontrolne.cpp
+++ b/flight/modules/PathFollower/pidcontrolne.cpp
@@ -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;
 }
 
diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml
index 6bb1314dc..223fa169f 100644
--- a/shared/uavobjectdefinition/flightmodesettings.xml
+++ b/shared/uavobjectdefinition/flightmodesettings.xml
@@ -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"/>