From ceef10cc418ace91fa03907d646ac446c28d5cb2 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Mon, 12 May 2014 14:56:11 -0700
Subject: [PATCH] AP_Buffer: Add peek_mutable function allowing in-place
 modification of buffered objects

---
 libraries/AP_Buffer/AP_Buffer.h | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/libraries/AP_Buffer/AP_Buffer.h b/libraries/AP_Buffer/AP_Buffer.h
index f8b8c1149c..30d0a335cc 100644
--- a/libraries/AP_Buffer/AP_Buffer.h
+++ b/libraries/AP_Buffer/AP_Buffer.h
@@ -38,6 +38,8 @@ public:
     /// @return
     const T& peek(uint8_t position) const;
 
+    T& peek_mutable(uint8_t position);
+
     /// front - return a reference to the element at the begin of the queue (i.e. the oldest element)
     /// If the queue is empty, 0 is returned.
     /// @return : oldest element
@@ -142,4 +144,17 @@ const T& AP_Buffer<T,SIZE>::peek(uint8_t position) const
     return _buff[j];
 }
 
+template <class T, uint8_t SIZE>
+T& AP_Buffer<T,SIZE>::peek_mutable(uint8_t position)
+{
+    uint8_t j = _head + position;
+
+    // wrap around if necessary
+    if( j >= SIZE )
+        j -= SIZE;
+
+    // return desired value
+    return _buff[j];
+}
+
 #endif  // __AP_BUFFER_H__