Implement a (Firmware) Circular Buffer (#1442)

* Add circular buffer skeleton with push and rudimentary get_index methods

* Add implmenetation with error catching on get_index

* Add comment headers in header file

* Fix formatting

* Fix comment

* Fix functions to resemble fw standards

* Fix minor cbuffer comments

* Clean up comments and variable names

* Fix assert statement

* Test death assertions and remove destroy()

* Add front method and tests, clean up variable names

* Remove assertion with negative for uint8

* Re-add circularbufferdestroy

* Add initzero test and nitpicks

* Change to size_t head and tail

* Add test for circular_buffer_create init 0

* Add @pre

* Remove stdint

* Add stdbool

* Rephrase comment in header

* Add empty to tests

* Fix comments

Author: kelvinkoon

src/firmware/shared/BUILD

@@ -27,3 +27,21 @@ cc_library(
         "//firmware/shared/math:tbots_math",
     ],
 )
+
+cc_library(
+    name = "circular_buffer",
+    srcs = ["circular_buffer.c"],
+    hdrs = ["circular_buffer.h"],
+    deps = [
+        "//shared:constants",
+    ],
+)
+
+cc_test(
+    name = "circular_buffer_test",
+    srcs = ["circular_buffer_test.cpp"],
+    deps = [
+        ":circular_buffer",
+        "@gtest//:gtest_main",
+    ],
+)

src/firmware/shared/circular_buffer.c

@@ -0,0 +1,88 @@
+#include "firmware/shared/circular_buffer.h"
+
+#include <assert.h>
+
+struct CircularBuffer
+{
+    size_t head;
+    size_t tail;
+    size_t max_size;
+    float *buffer_data;
+    bool is_full;
+};
+
+CircularBuffer_t *circular_buffer_create(size_t size)
+{
+    // Check if size is greater than 0
+    assert(size > 0);
+
+    CircularBuffer_t *new_cbuffer = malloc(sizeof(CircularBuffer_t));
+
+    new_cbuffer->head        = 0;
+    new_cbuffer->tail        = 0;
+    new_cbuffer->max_size    = size;
+    new_cbuffer->buffer_data = (float *)malloc(sizeof(float) * new_cbuffer->max_size);
+    new_cbuffer->is_full     = false;
+
+    return new_cbuffer;
+}
+
+void circular_buffer_destroy(CircularBuffer_t *cbuffer)
+{
+    free(cbuffer->buffer_data);
+    free(cbuffer);
+}
+
+void circular_buffer_push(CircularBuffer_t *cbuffer, float data)
+{
+    cbuffer->buffer_data[cbuffer->head] = data;
+    cbuffer->head                       = ++(cbuffer->head) % cbuffer->max_size;
+    if (circular_buffer_isFull(cbuffer) == true)
+    {
+        cbuffer->tail = ++(cbuffer->tail) % cbuffer->max_size;
+    }
+
+    // Implementation assumes head and tail are equal when full
+    if (cbuffer->head == cbuffer->tail)
+    {
+        cbuffer->is_full = true;
+    }
+}
+
+float circular_buffer_getAtIndex(CircularBuffer_t *cbuffer, size_t index)
+{
+    // Check if index is larger than the maximum buffer size
+    assert(index <= cbuffer->max_size);
+
+    // Check if enough items are in the buffer for index
+    assert(circular_buffer_isFull(cbuffer) == true || index <= cbuffer->head);
+
+    int requested_index = (cbuffer->head % cbuffer->max_size) - index - 1;
+
+    // Check if requestedIndex needs to be looped
+    if (requested_index < 0)
+    {
+        requested_index = cbuffer->max_size + requested_index;
+    }
+    float res = cbuffer->buffer_data[requested_index];
+    return res;
+}
+
+float circular_buffer_front(CircularBuffer_t *cbuffer)
+{
+    // Check if buffer is empty
+    assert(!circular_buffer_isEmpty(cbuffer));
+
+    float res = circular_buffer_getAtIndex(cbuffer, 0);
+    return res;
+}
+
+bool circular_buffer_isFull(CircularBuffer_t *cbuffer)
+{
+    return cbuffer->is_full;
+}
+
+bool circular_buffer_isEmpty(CircularBuffer_t *cbuffer)
+{
+    return (circular_buffer_isFull(cbuffer) == false && cbuffer->head == 0);
+}

src/firmware/shared/circular_buffer.h

@@ -0,0 +1,77 @@
+#include <stdbool.h>
+#include <stdlib.h>
+
+/**
+ * This struct represents the circular buffer properties
+ */
+typedef struct CircularBuffer CircularBuffer_t;
+
+/**
+ * Initialize the circular_buffer struct members
+ *
+ * @param size Size of the circular_buffer to set
+ *
+ * @pre: `size` must be larger than 0
+ *
+ * @return A pointer to the created circular_buffer, ownership is given to the caller
+ */
+CircularBuffer_t *circular_buffer_create(size_t size);
+
+/**
+ * Destroy the circular_buffer, freeing any memory allocated for it
+ *
+ * @param cbuffer The circular_buffer to destroy
+ */
+void circular_buffer_destroy(CircularBuffer_t *cbuffer);
+
+/**
+ * Push data into the circular_buffer
+ * If full, will overwrite the oldest value
+ *
+ * @param cbuffer The circular_buffer
+ * @param data  Value to be inserted into the circular_buffer
+ */
+void circular_buffer_push(CircularBuffer_t *cbuffer, float data);
+
+/**
+ * Retrieve a relative recent value in the circular_buffer
+ *
+ * @param cbuffer The circular_buffer
+ * @param index Index relative to the most recent value in the buffer
+ *
+ * @pre: `index` must be less than the max_size of the circular_buffer
+ *
+ * @return  Data from the index which is relative to the most recent value in the buffer.
+ * Eg. 0 is the most recent value, 1 is the second most recent, etc
+ */
+float circular_buffer_getAtIndex(CircularBuffer_t *cbuffer, size_t index);
+
+/**
+ * Retrieve the most recent value in the circular_buffer
+ * Retrieved value is not destroyed or removed from circular_buffer
+ *
+ * @param cbuffer The circular_buffer
+ *
+ * @pre: At least one item must be in the circular_buffer
+ *
+ * @return The most recent value
+ */
+float circular_buffer_front(CircularBuffer_t *cbuffer);
+
+/**
+ * Return if the circular_buffer is full
+ *
+ * @param cbuffer The circular_buffer
+ *
+ * @return A boolean indicating if the circular_buffer is full
+ */
+bool circular_buffer_isFull(CircularBuffer_t *cbuffer);
+
+/**
+ * Return if the circular_buffer is empty
+ *
+ * @param cbuffer The circular_buffer
+ *
+ * @return A boolean indicating if the circular_buffer is empty
+ */
+bool circular_buffer_isEmpty(CircularBuffer_t *cbuffer);