From 4c6bed182c62de813d68b4d4cafd396479347691 Mon Sep 17 00:00:00 2001
From: Jan Lehnardt <jan@apache.org>
Date: Sat, 10 Nov 2012 15:42:29 +0000
Subject: [PATCH] send FTRIM before takeoff to stabilize

---
 lib/Client.js                   | 28 ++++++++++++++++++++++++++++
 lib/control/AtCommandCreator.js |  5 +++++
 2 files changed, 33 insertions(+)

diff --git a/lib/Client.js b/lib/Client.js
index 839aec1..aa82b08 100644
--- a/lib/Client.js
+++ b/lib/Client.js
@@ -137,6 +137,17 @@ Client.prototype.disableEmergency = function() {
 };
 
 Client.prototype.takeoff = function() {
+  this.ftrim();
+  var self = this;
+  setTimeout(function() {
+    console.log("Take-off");
+    self._ref.fly = true;
+  }, 5000);
+  return true;
+};
+
+Client.prototype.takeoffUnstable = function() {
+  console.log("Take-off Unstable");
   this._ref.fly = true;
   return true;
 };
@@ -160,6 +171,23 @@ Client.prototype.config = function(key, value) {
   });
 };
 
+
+Client.prototype.ftrim = function() {
+  // @TODO Figure out if we can get a ACK for this, so we don't need to
+  // repeat it blindly like this
+
+  if(this._ref.fly) {
+    console.log("You can’t ftrim when you fly");
+    return false;
+  }
+
+  var self = this;
+  console.log("Stabilizing…")
+  this._repeat(100, function() {
+    self._udpControl.ftrim();
+  });
+};
+
 Client.prototype.animate = function(animation, duration) {
   // @TODO Figure out if we can get a ACK for this, so we don't need to
   // repeat it blindly like this
diff --git a/lib/control/AtCommandCreator.js b/lib/control/AtCommandCreator.js
index f929fa7..da786d8 100644
--- a/lib/control/AtCommandCreator.js
+++ b/lib/control/AtCommandCreator.js
@@ -31,6 +31,11 @@ AtCommandCreator.prototype.ref = function(options) {
   return this.raw('REF', args);
 };
 
+// TMP: Used to send FTRIM
+AtCommandCreator.prototype.ftrim = function() {
+  this.raw('FTRIM');
+}
+
 // Used to fly the drone around
 AtCommandCreator.prototype.pcmd = function(options) {
   options = options || {};