completed TM circ_queue

Drivers/common/circular_buffer/inc/circular_buffer.hpp

@@ -62,6 +62,13 @@ class CircularBuffer {
  */
  uint16_t getFreeSpaceBytes();
 
+ /**
+ * @brief Provide the size of the buffer.
+ *
+ * @return uint16_t The size of the buffer.
+ */
+ uint16_t getSize_();
+
  private:
  uint8_t* buf_;
  uint16_t size_;

Drivers/common/circular_buffer/src/circular_buffer.cpp

@@ -69,4 +69,8 @@ bool CircularBuffer::write(uint8_t byte) {
 
 uint16_t CircularBuffer::getFreeSpaceBytes() { 
  //NOT IMPLEMENTED YET
- return -1; }
+ return -1; }
+
+uint16_t CircularBuffer::getSize_(){
+ return size_;
+}

TelemetryManager/Inc/TMCircularBuffer.hpp

@@ -45,14 +45,14 @@ class TMCircularBuffer : public CircularBuffer {
  *
  * @return MAVLinkByte The byte that was dequeued
  */
- bool dequeue();
+ MAVLinkByte dequeue();
 
  /**
  * @brief Enqueue a byte into the queue
  *
  * @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 +65,7 @@ class TMCircularBuffer : public CircularBuffer {
  * in the MAVLink message.
  *
  */
- int bytesUntilLastMessageEnd();
+ int bytesUntilLastMessageEnd(uint8_t* buf);
 
  /**
  * @brief Returns the index of the current byte in the queue. This is useful for when we want to
@@ -76,6 +76,9 @@ class TMCircularBuffer : public CircularBuffer {
  * @return int The index of the current byte in the queue.
  */
  int currentIndex();
+
+ private: 
+ int index = 0;
 };
 
 #endif

TelemetryManager/Src/TMCircularBuffer.cpp

@@ -11,46 +11,46 @@ TMCircularBuffer::TMCircularBuffer(CircularBuffer* buf): CircularBuffer(*buf) {
 
 TMCircularBuffer::~TMCircularBuffer() {
  // Destructor
- delete[] buf;
- buf = nullptr;
+
  }
 
-TMCircularBuffer::bool TMCircularBuffer::dequeue(MAVLinkByte &byte) {
- if(getNumAvailBytes() - size_ == 0){
- return false;
- else{
- byte = buf[(readPtr_)];
- readPtr_ = (readPtr_ +1) % size_;
- --size_;
- return true;
+TMCircularBuffer::MAVLinkByte TMCircularBuffer::dequeue() {
+ uint8_t* res;
+ if(read(res,1)){
+ return *res;
  }
+ return *res;
+}
 
-
- }
-
-void TMCircularBuffer::enqueue(MAVLinkByte byte) {
+bool TMCircularBuffer::enqueue(MAVLinkByte byte) {
  // Enqueue the byte
- if(!isFull()){
- buf_[writePtr_ % size_] = byte;
- writePtr_++;
+ if(write(byte)){
+ index ++;
+ return true;
  }
+ return false;
 }
 
-int TMCircularBuffer::bytesUntilLastMessageEnd() {
+int TMCircularBuffer::bytesUntilLastMessageEnd(uint8_t* buf) {
  /*
  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.
  */
- uint16_t index = readPtr_;
- uint16_t count = 0;
- while(count < size_){
- if(buf_[index] ==0xFD){
- return index;
- }
- index = (index + 1) % size_;
- count ++;
- }
- return -1;
+ uint16_t index = 0;
+ uint16_t count = 0;
+ uint16_t size = getSize_();
+ while(count < size){
+ if( buf[index] == 0xFD){
+ return index;
+ }
+
+ index = (index + 1) % size;
+ count ++;
+
+ }
+
 }
 
-int TMCircularBuffer::currentIndex() { return readPtr_; }
+int TMCircularBuffer::currentIndex() {
+ return index;
+ }