Feature/circular queue (#55)

Drivers/common/circular_buffer/inc/circular_buffer.hpp

@@ -16,6 +16,16 @@ class CircularBuffer {
  */
  CircularBuffer(uint8_t* buf, uint16_t size);
 
+ /** @brief Copy Constructor for the CircularBuffer class
+ *
+ * CircularBuffer will create a copy of a stack-allocated byte array as a
+ * circular buffer.
+ *
+ * .
+ * @param other an instance of a circula r buffer 
+ */
+ CircularBuffer(const CircularBuffer& other);
+
  /** @brief Read the value of a byte in the circular buffer
  * without incrementing the read pointer
  *
@@ -62,7 +72,8 @@ class CircularBuffer {
  */
  uint16_t getFreeSpaceBytes();
 
- private:
+
+ protected:
  uint8_t* buf_;
  uint16_t size_;
  uint16_t writePtr_ = 0;

Drivers/common/circular_buffer/src/circular_buffer.cpp

@@ -7,6 +7,10 @@ CircularBuffer::CircularBuffer(uint8_t* buf, uint16_t size) {
  size_ = size;
 }
 
+CircularBuffer::CircularBuffer(const CircularBuffer& other) {
+
+}
+
 bool CircularBuffer::peek(uint8_t& byte, uint16_t index) {
  bool success = true;
 
@@ -69,4 +73,4 @@ bool CircularBuffer::write(uint8_t byte) {
 
 uint16_t CircularBuffer::getFreeSpaceBytes() { 
  //NOT IMPLEMENTED YET
- return -1; }
+ return -1; }

TelemetryManager/Inc/TMCircularBuffer.hpp

@@ -14,6 +14,7 @@
 
 #ifndef TMCIRCULARBUFFER_H
 #define TMCIRCULARBUFFER_H
+#define MAX_MAVLINK_MSG_SIZE 280
 
 #include "circular_buffer.hpp"
 
@@ -31,7 +32,7 @@ class TMCircularBuffer : public CircularBuffer {
  * @brief Construct a new Circular Buffer object. Do whatever needs to be done here.
  *
  */
- TMCircularBuffer(CircularBuffer* buf);
+ TMCircularBuffer(CircularBuffer* buf );
 
  /**
  * @brief Destroy and cleanup memory (everything should be static anyways). Do whatever
@@ -52,7 +53,7 @@ class TMCircularBuffer : public CircularBuffer {
  *
  * @param byte The byte to be enqueued
  */
- void enqueue(MAVLinkByte byte);
+ bool enqueue(MAVLinkByte byte);
 
  /**
  * @brief Get the index of the last full message in the queue determined by the end flag
@@ -65,7 +66,7 @@ class TMCircularBuffer : public CircularBuffer {
  * in the MAVLink message.
  *
  */
- int lastFullMessageEndIndex();
+ int bytesUntilLastMessageEnd();
 
  /**
  * @brief Returns the index of the current byte in the queue. This is useful for when we want to
@@ -75,7 +76,9 @@ class TMCircularBuffer : public CircularBuffer {
  *
  * @return int The index of the current byte in the queue.
  */
- int currentIndex();
+ uint16_t currentIndex();
+
+
 };
 
 #endif

TelemetryManager/Src/TMCircularBuffer.cpp

@@ -1,30 +1,55 @@
 
-#include "CircularBuffer.hpp"
+#include "TMCircularBuffer.hpp"
 
-TMCircularBuffer::TMCircularBuffer(uint8_t* buf, uint16_t size): CircularBuffer(buf, size) {
+TMCircularBuffer::TMCircularBuffer(uint8_t* buf, uint16_t size): CircularBuffer(*buf, size) {
  // Constructor
 }
 
-TMCircularBuffer::TMCircularBuffer(CircularBuffer buf): CircularBuffer(buf) {
+TMCircularBuffer::TMCircularBuffer(CircularBuffer* buf ): CircularBuffer(*buf) {
  // Constructor
-}
+ }
 
 TMCircularBuffer::~TMCircularBuffer() {
  // Destructor
+
+ }
+
+TMCircularBuffer::MAVLinkByte TMCircularBuffer::dequeue() {
+ MAVLinkByte* res;
+ if(read(res,1)){
+ return *res;
+ }
+ return 0;
 }
 
-TMCircularBuffer::MAVLinkByte TMCircularBuffer::dequeue() { return 0; }
-
-void TMCircularBuffer::enqueue(MAVLinkByte byte) {
+bool TMCircularBuffer::enqueue(MAVLinkByte byte) {
  // Enqueue the byte
+ if(write(byte)){
+ return true;
+ }
+ return false;
 }
 
-int TMCircularBuffer::lastFullMessageEndIndex() {
+int TMCircularBuffer::bytesUntilLastMessageEnd() {
  /*
- Rahul: This one is a bit tricky cause you need to know the structure of the MAVLink message. 
+ Rahul: This one is a bit tricky because you need to know the structure of the MAVLink message. 
  I can help you with this one if you want.
  */
- return -1;
+ int index = -1;
+ uint16_t count = 0;
+ uint16_t size = size_;
+ while(count < size){
+ if( buf_[index] == 0xFD){
+ return index;
+ }
+
+ index = (index + 1) % size;
+ count ++;
+ }
+ return index;
+
 }
 
-int TMCircularBuffer::currentIndex() { return -1; }
+uint16_t TMCircularBuffer::currentIndex() {
+ return readPtr_;
+ }