From 17893958bf07b579c69ddc100c0718345a6ac993 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 16 May 2017 13:55:35 +1000
Subject: [PATCH] HAL_PX4: cope with brushed motors in read_last_sent()

we need the value before scaling is applied, so slew rates can work
---
 libraries/AP_HAL_PX4/RCOutput.cpp | 9 ++++++++-
 libraries/AP_HAL_PX4/RCOutput.h   | 3 +++
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
index 57f3abe659..506994fff6 100644
--- a/libraries/AP_HAL_PX4/RCOutput.cpp
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -73,6 +73,7 @@ void PX4RCOutput::init()
     // ensure not to write zeros to disabled channels
     for (uint8_t i=0; i < PX4_NUM_OUTPUT_CHANNELS; i++) {
         _period[i] = PWM_IGNORE_THIS_CHANNEL;
+        _last_sent[i] = PWM_IGNORE_THIS_CHANNEL;
     }
 }
 
@@ -229,6 +230,7 @@ void PX4RCOutput::enable_ch(uint8_t ch)
     _enabled_channels |= (1U<<ch);
     if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
         _period[ch] = 0;
+        _last_sent[ch] = 0;
     }
 }
 
@@ -329,7 +331,12 @@ void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
         _max_channel = ch + 1;
     }
 
+
     if (_output_mode == MODE_PWM_BRUSHED) {
+
+        // keep unscaled value
+        _last_sent[ch] = period_us;
+    
         // map from the PWM range to 0 t0 100% duty cycle. For 16kHz
         // this ends up being 0 to 500 pulse width in units of
         // 125usec.
@@ -386,7 +393,7 @@ uint16_t PX4RCOutput::read_last_sent(uint8_t ch)
         return 0;
     }
 
-    return _period[ch];
+    return _last_sent[ch];
 }
 
 void PX4RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h
index ab34928aa1..37038b7d10 100644
--- a/libraries/AP_HAL_PX4/RCOutput.h
+++ b/libraries/AP_HAL_PX4/RCOutput.h
@@ -45,6 +45,9 @@ private:
     int _alt_fd;
     uint16_t _freq_hz;
     uint16_t _period[PX4_NUM_OUTPUT_CHANNELS];
+    // we keep the last_sent value separately, as we need to keep the unscaled
+    // value for systems with brushed motors which scale outputs
+    uint16_t _last_sent[PX4_NUM_OUTPUT_CHANNELS];
     volatile uint8_t _max_channel;
     volatile bool _need_update;
     bool _sbus_enabled:1;