From 082c1f77fbe39a0fa36f123a65a4dbc05a6adcee Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 19 Oct 2015 17:56:49 -0700
Subject: [PATCH] Copter: soften takeoff

---
 ArduCopter/Copter.h    |  4 ++--
 ArduCopter/takeoff.cpp | 28 ++++++++++++++++++++--------
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index e7246b8fca..71e59e1f96 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -260,9 +260,9 @@ private:
 
     struct {
         bool running;
-        float speed;
+        float max_speed;
+        float alt_delta;
         uint32_t start_ms;
-        uint32_t time_ms;
     } takeoff_state;
 
     RCMapper rcmap;
diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp
index f1a664a390..0f733044b3 100644
--- a/ArduCopter/takeoff.cpp
+++ b/ArduCopter/takeoff.cpp
@@ -66,9 +66,9 @@ void Copter::takeoff_timer_start(float alt_cm)
 
     // initialise takeoff state
     takeoff_state.running = true;
-    takeoff_state.speed = speed;
+    takeoff_state.max_speed = speed;
     takeoff_state.start_ms = millis();
-    takeoff_state.time_ms = (alt_cm/takeoff_state.speed) * 1.0e3f;
+    takeoff_state.alt_delta = alt_cm;
 }
 
 // stop takeoff
@@ -90,21 +90,33 @@ void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_cli
         return;
     }
 
-    // check if timeout as expired
-    if ((millis()-takeoff_state.start_ms) >= takeoff_state.time_ms) {
+    float takeoff_minspeed = min(50.0f,takeoff_state.max_speed);
+    static const float takeoff_accel = 50.0f;
+    float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f;
+    float speed = min(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed);
+
+    float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel;
+    float height_gained;
+    if (time_elapsed <= time_to_max_speed) {
+        height_gained = 0.5f*takeoff_accel*sq(time_elapsed) + takeoff_minspeed*time_elapsed;
+    } else {
+        height_gained = 0.5f*takeoff_accel*sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed +
+                        (time_elapsed-time_to_max_speed)*takeoff_state.max_speed;
+    }
+
+    // check if the takeoff is over
+    if (height_gained >= takeoff_state.alt_delta) {
         takeoff_stop();
-        takeoff_climb_rate = 0.0f;
-        return;
     }
 
     // if takeoff climb rate is zero return
-    if (takeoff_state.speed <= 0.0f) {
+    if (speed <= 0.0f) {
         takeoff_climb_rate = 0.0f;
         return;
     }
 
     // default take-off climb rate to maximum speed
-    takeoff_climb_rate = takeoff_state.speed;
+    takeoff_climb_rate = speed;
 
     // if pilot's commands descent
     if (pilot_climb_rate < 0.0f) {