From 0e49ee03351e776b9112c163fdc367011f0cfacb Mon Sep 17 00:00:00 2001
From: myles-parfeniuk <myles.parfenyuk@gmail.com>
Date: Mon, 22 Jul 2024 23:16:14 -0700
Subject: [PATCH] new feature, data cbs

---
 BNO08x.cpp | 23 ++++++++++++++++++++---
 BNO08x.hpp |  9 +++++++++
 2 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/BNO08x.cpp b/BNO08x.cpp
index 6537d9d..c6d880d 100644
--- a/BNO08x.cpp
+++ b/BNO08x.cpp
@@ -310,7 +310,7 @@ bool BNO08x::soft_reset()
 }
 
 /**
- * @brief Requests product ID, prints the returned info over serial, and returns the reason for the most resent reset. 
+ * @brief Requests product ID, prints the returned info over serial, and returns the reason for the most resent reset.
  *
  * @return The reason for the most recent recent reset ( 1 = POR (power on reset), 2 = internal reset, 3 = watchdog
  * timer, 4 = external reset 5 = other)
@@ -328,7 +328,7 @@ uint8_t BNO08x::get_reset_reason()
     {
         // receive product ID report
         if (wait_for_data())
-            xQueueReceive(queue_reset_reason, &reset_reason, HOST_INT_TIMEOUT_MS/portTICK_PERIOD_MS);
+            xQueueReceive(queue_reset_reason, &reset_reason, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS);
         else
             ESP_LOGE(TAG, "Failed to receive product ID report.");
     }
@@ -850,6 +850,17 @@ bool BNO08x::data_available()
     return wait_for_data();
 }
 
+/**
+ * @brief Registers a callback to execute when new data from a report is received.
+ *
+ * @param cb_fxn Pointer to the call-back function should be of void return type and void input parameters. 
+ * @return void, nothing to return
+ */
+void BNO08x::register_cb(std::function<void()> cb_fxn)
+{
+    cb_list.push_back(cb_fxn);
+}
+
 /**
  * @brief Parses a packet received from bno08x, updating any data according to received reports.
  *
@@ -2810,8 +2821,14 @@ void BNO08x::data_proc_task()
     {
         if (xQueueReceive(queue_rx_data, &packet, portMAX_DELAY)) // receive packet from spi_task()
         {
-            if (parse_packet(&packet) != 0)                                   // check if packet is valid
+            if (parse_packet(&packet) != 0) // check if packet is valid
+            {
+                //execute any registered callbacks 
+                for(auto& cb_fxn : cb_list)
+                    cb_fxn(); 
+
                 xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET); // indicate valid packet to wait_for_data()
+            }
             else
                 xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_INVALID_PACKET); // indicated invalid packet to wait_for_data()
         }
diff --git a/BNO08x.hpp b/BNO08x.hpp
index 872a23a..b5ecfe0 100644
--- a/BNO08x.hpp
+++ b/BNO08x.hpp
@@ -1,4 +1,5 @@
 #pragma once
+//esp-idf includes
 #include <driver/gpio.h>
 #include <driver/spi_common.h>
 #include <driver/spi_master.h>
@@ -11,10 +12,13 @@
 #include <freertos/queue.h>
 #include <rom/ets_sys.h>
 
+//standard library includes
 #include <inttypes.h>
 #include <math.h>
 #include <stdio.h>
 #include <cstring>
+#include <functional>
+#include <vector>
 
 /// @brief SHTP protocol channels
 enum channels_t
@@ -185,6 +189,7 @@ class BNO08x
         void clear_tare();
 
         bool data_available();
+        void register_cb(std::function<void()> cb_fxn);
 
         uint32_t get_time_stamp();
 
@@ -349,6 +354,8 @@ class BNO08x
 
         static bno08x_config_t default_imu_config; ///< default imu config settings
 
+
+
         EventGroupHandle_t
                 evt_grp_spi; ///<Event group for indicating when bno08x hint pin has triggered and when new data has been processed. Used by calls to sending or receiving functions.
         EventGroupHandle_t evt_grp_report_en; ///<Event group for indicating which reports are currently enabled.
@@ -358,6 +365,8 @@ class BNO08x
         QueueHandle_t queue_frs_read_data; ///<Queue used to send packet body from data_proc_task to frs read functions.
         QueueHandle_t queue_reset_reason; ///<Queue used to send reset reason from product id report to reset_reason() function
 
+        std::vector<std::function<void()>> cb_list; //Vector for storing any call-back functions added with register_cb()
+
         uint32_t meta_data[9]; ///<First 9 bytes of meta data returned from FRS read operation (we don't really need the rest) (See Ref. Manual 5.1)
 
         bno08x_config_t imu_config{};                   ///<IMU configuration settings