From fa24107a2afedec6edaa29bb6d51e8e2dd762a34 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Mon, 10 Aug 2015 17:24:57 -0400
Subject: [PATCH] AP_MotorsHeli_RSC: Split out rotor speed estimate into it's
 own function

---
 libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 15 ++++++++++++---
 libraries/AP_Motors/AP_MotorsHeli_RSC.h   |  3 +++
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
index 7685594fc5..fe195aade5 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
+++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
@@ -65,9 +65,12 @@ void AP_MotorsHeli_RSC::output(uint8_t state)
             break;
     }
 
-    // run speed ramp function to slew output smoothly, also updates estimated speed
+    // run speed ramp function to slew output smoothly
     speed_ramp(_control_speed);
 
+    // update rotor speed estimate
+    update_speed_estimate();
+
     // output to rsc servo
     write_rsc(_control_out);
 }
@@ -94,6 +97,12 @@ void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target)
         _control_out = speed_target;
     }
 
+
+}
+
+// update_speed_estimate - function to estimate speed
+void AP_MotorsHeli_RSC::update_speed_estimate()
+{
     // ramp speed estimate towards control out
     if (_estimated_speed < _control_out) {
         _estimated_speed += _runup_increment;
@@ -107,8 +116,8 @@ void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target)
         }
     }
 
-    // set runup complete flag
-    if (!_runup_complete && speed_target > 0 && _estimated_speed >= speed_target) {
+    // update run-up complete flag
+    if (!_runup_complete && _control_out > _idle_speed && _estimated_speed >= _control_out) {
         _runup_complete = true;
     }
     if (_runup_complete && _estimated_speed <= _critical_speed) {
diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h
index 35feacc6f4..4d8a605683 100644
--- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h
+++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h
@@ -81,6 +81,9 @@ private:
     // speed_ramp - ramps speed towards target, result put in _control_out
     void            speed_ramp(int16_t rotor_target);
 
+    // update_speed_estimate - function to estimate speed
+    void            update_speed_estimate();
+
     // write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000
     void            write_rsc(int16_t servo_out);
 };