From 4bf3ec0e91d1ab4c886337d154b2b7c2d6492412 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Thu, 10 Mar 2016 13:55:58 -0800
Subject: [PATCH] AP_HAL_PX4: implement RCOutput::read_last_sent

---
 libraries/AP_HAL_PX4/RCOutput.cpp | 16 ++++++++++++++++
 libraries/AP_HAL_PX4/RCOutput.h   |  2 ++
 2 files changed, 18 insertions(+)

diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
index c8056da655..6a73dae03e 100644
--- a/libraries/AP_HAL_PX4/RCOutput.cpp
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -285,6 +285,22 @@ void PX4RCOutput::read(uint16_t* period_us, uint8_t len)
     }
 }
 
+uint16_t PX4RCOutput::read_last_sent(uint8_t ch)
+{
+    if (ch >= PX4_NUM_OUTPUT_CHANNELS) {
+        return 0;
+    }
+
+    return _period[ch];
+}
+
+void PX4RCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
+{
+    for (uint8_t i=0; i<len; i++) {
+        period_us[i] = read_last_sent(i);
+    }
+}
+
 /*
   update actuator armed state
  */
diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h
index 07ddee118a..35899c1cd7 100644
--- a/libraries/AP_HAL_PX4/RCOutput.h
+++ b/libraries/AP_HAL_PX4/RCOutput.h
@@ -20,6 +20,8 @@ public:
     void     write(uint8_t ch, uint16_t period_us) override;
     uint16_t read(uint8_t ch) override;
     void     read(uint16_t* period_us, uint8_t len) override;
+    uint16_t read_last_sent(uint8_t ch) override;
+    void     read_last_sent(uint16_t* period_us, uint8_t len) override;
     void     set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
     void     set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
     bool     force_safety_on(void) override;