Diff: imu_driver.hpp

Revision:

15:ca49fdec90fc

Parent:

14:6c31a7ab45f0

Child:

16:6583ad08d0d0

--- a/imu_driver.hpp	Tue Dec 03 14:12:48 2019 +0000
+++ b/imu_driver.hpp	Tue Dec 03 14:18:18 2019 +0000
@@ -187,14 +187,14 @@
         if (m_extiConfig & ImuExtiRcvNormalMsg) {
             receiveBurstMsgImpl();
         }
-        
+
         if (m_extiConfig & ImuExtiRcvAhrsMsg) {
             receiveAhrsMsg();
         }
     }
 private:
     ImuDriverStatus receiveBurstMsgImpl(bool t_extended = false)
-    {   
+    {
         if (m_spiIsDataReady()) {
             std::uint16_t max_data = 0U;
             std::uint16_t burst_reg = 0U;
@@ -247,6 +247,47 @@
             return ImuDriverStatusDataNotReady;
         }
     }
+
+    ImuDriverStatus receiveAhrsMsgImpl()
+    {
+        if (m_spiIsDataReady()) {
+            static std::uint8_t data_rcved = 0U;
+            const std::uint8_t max_data = 5U;
+            const std::uint16_t ahrs_reg = m_spiGenerateReadCmd(ReadAhrsBurstDataRegister);
+
+            m_ss.write(0);
+            m_imuSpiWrite(ahrs_reg);
+
+            while(data_rcved < max_data) {
+                const std::uint16_t spi_data = m_imuSpiWrite(0x0000);
+                switch(data_rcved) {
+                    case 0:
+                        ahrsRawData.status = spi_data;
+                        break;
+                    case 1:
+                    case 2:
+                    case 3:
+                        ahrsRawData.attitude[data_rcved - 1] = spi_data;
+                        break;
+                    case 4:
+                        ahrsRawData.temperature = spi_data;
+                        break;
+                    default:
+                        break;
+                }
+
+                ++data_rcved;
+            }
+
+            data_rcved = 0U;
+            m_ss.write(1);
+
+            m_processAhrsRawData();
+            return ImuDriverStatusOK;
+        } else {
+            return ImuDriverStatusDataNotReady;
+        }
+    }
 public:
     /**
      *  imuRawData contains raw data received from @ref receiveBurstMsg(bool t_extended)
@@ -296,8 +337,9 @@
      *
      *  @return ImuDriverStatus to indicate the result of the receive operation
      */
-    ImuDriverStatus receiveBurstMsg(bool t_extended = false) {
-        return (m_extiConfig & ImuExtiRcvNormalMsg ? ImuDriverStatusInvalidCall : receiveBurstMsg(t_extended));
+    ImuDriverStatus receiveBurstMsg(bool t_extended = false)
+    {
+        return (m_extiConfig & ImuExtiRcvNormalMsg ? ImuDriverStatusInvalidCall : receiveBurstMsgImpl(t_extended));
     }
 
     /**
@@ -311,46 +353,11 @@
      */
     ImuDriverStatus receiveAhrsMsg()
     {
-        if (m_spiIsDataReady()) {
-            static std::uint8_t data_rcved = 0U;
-            const std::uint8_t max_data = 5U;
-            const std::uint16_t ahrs_reg = m_spiGenerateReadCmd(ReadAhrsBurstDataRegister);
-
-            m_ss.write(0);
-            m_imuSpiWrite(ahrs_reg);
+        return (m_extiConfig & ImuExtiRcvAhrsMsg ? ImuDriverStatusInvalidCall : receiveAhrsMsgImpl());
+    }
 
-            while(data_rcved < max_data) {
-                const std::uint16_t spi_data = m_imuSpiWrite(0x0000);
-                switch(data_rcved) {
-                    case 0:
-                        ahrsRawData.status = spi_data;
-                        break;
-                    case 1:
-                    case 2:
-                    case 3:
-                        ahrsRawData.attitude[data_rcved - 1] = spi_data;
-                        break;
-                    case 4:
-                        ahrsRawData.temperature = spi_data;
-                        break;
-                    default:
-                        break;
-                }
-
-                ++data_rcved;
-            }
-
-            data_rcved = 0U;
-            m_ss.write(1);
-
-            m_processAhrsRawData();
-            return ImuDriverStatusOK;
-        } else {
-            return ImuDriverStatusDataNotReady;
-        }
-    }
-    
-    ImuDriverStatus configExti(const ImuExtiConfig t_exti_config) {
+    ImuDriverStatus configExti(const ImuExtiConfig t_exti_config)
+    {
         m_extiConfig = t_exti_config;
     }
 };