From 77f6ac57e959698dfcf77ffbad643b93ee0a2c89 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Tue, 30 Nov 2021 16:45:33 +0100
Subject: [PATCH] some reaction wheel fixes

---
 bsp_q7s/callbacks/rwSpiCallback.cpp | 15 +++++++++++++--
 bsp_q7s/callbacks/rwSpiCallback.h   |  3 +++
 bsp_q7s/core/InitMission.cpp        |  2 +-
 mission/devices/RwHandler.h         |  2 ++
 4 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/bsp_q7s/callbacks/rwSpiCallback.cpp b/bsp_q7s/callbacks/rwSpiCallback.cpp
index 620f63f7..ad99d505 100644
--- a/bsp_q7s/callbacks/rwSpiCallback.cpp
+++ b/bsp_q7s/callbacks/rwSpiCallback.cpp
@@ -133,8 +133,15 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sen
             closeSpi(gpioId, gpioIF, mutex);
             return RwHandler::SPI_READ_FAILURE;
         }
+        if(idx == 0) {
+            if(byteRead != FLAG_BYTE) {
+                sif::error << "Invalid data, expected start marker" << std::endl;
+                closeSpi(gpioId, gpioIF, mutex);
+                return RwHandler::NO_START_MARKER;
+            }
+        }
 
-        if (byteRead != 0x7E) {
+        if (byteRead != FLAG_BYTE) {
             break;
         }
 
@@ -145,6 +152,10 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sen
         }
     }
 
+#if FSFW_HAL_SPI_WIRETAPPING == 1
+    sif::info << "RW start marker detected" << std::endl;
+#endif
+
     size_t decodedFrameLen = 0;
     while(decodedFrameLen < replyBufferSize) {
 
@@ -158,7 +169,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sen
             }
         }
 
-        if (byteRead == 0x7E) {
+        if (byteRead == FLAG_BYTE) {
             /** Reached end of frame */
             break;
         }
diff --git a/bsp_q7s/callbacks/rwSpiCallback.h b/bsp_q7s/callbacks/rwSpiCallback.h
index cc7c6cbe..8952f873 100644
--- a/bsp_q7s/callbacks/rwSpiCallback.h
+++ b/bsp_q7s/callbacks/rwSpiCallback.h
@@ -8,6 +8,9 @@
 
 namespace rwSpiCallback {
 
+//! This is the end and start marker of the frame datalinklayer
+static constexpr uint8_t FLAG_BYTE = 0x7E;
+
 /**
  * @brief   This is the callback function to send commands to the nano avionics reaction wheels and
  *          receive the replies.
diff --git a/bsp_q7s/core/InitMission.cpp b/bsp_q7s/core/InitMission.cpp
index 7923a2cd..81d972b1 100644
--- a/bsp_q7s/core/InitMission.cpp
+++ b/bsp_q7s/core/InitMission.cpp
@@ -22,7 +22,7 @@
 ServiceInterfaceStream sif::debug("DEBUG");
 ServiceInterfaceStream sif::info("INFO");
 ServiceInterfaceStream sif::warning("WARNING");
-ServiceInterfaceStream sif::error("ERROR", false, false, true);
+ServiceInterfaceStream sif::error("ERROR");
 #else
 ServiceInterfaceStream sif::debug("DEBUG", true);
 ServiceInterfaceStream sif::info("INFO", true);
diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h
index cd753be0..57895ca6 100644
--- a/mission/devices/RwHandler.h
+++ b/mission/devices/RwHandler.h
@@ -46,6 +46,8 @@ public:
     static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4);
     //! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames.
     static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5);
+    //! [EXPORT] : [COMMENT] Expected a start marker as first byte
+    static const ReturnValue_t NO_START_MARKER = MAKE_RETURN_CODE(0xB6);
 
 protected:
 	void doStartUp() override;