DONE

Author: RobertPendergrast

FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp

@@ -157,15 +157,15 @@ void CameraHandler ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
 }
 
 void CameraHandler ::PING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
+    if (this->m_receiving) {
+        this->log_WARNING_LO_FailedCommandCurrentlyReceiving();
+        return;
+    }
+    this->m_waiting_for_pong = true;
     const char* pingCmd = "ping";
     SEND_COMMAND_cmdHandler(opCode, cmdSeq, Fw::CmdStringArg(pingCmd));
 }
 
-void CameraHandler ::SET_IMAGE_QUALITY_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
-    const char* takeImageCmd = "snap";
-    SEND_COMMAND_cmdHandler(opCode, cmdSeq, Fw::CmdStringArg(takeImageCmd));
-}
-
 void CameraHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) {
     // Append newline to command to send to PayloadCom
     Fw::CmdStringArg tempCmd = cmd;  
@@ -184,7 +184,6 @@ void CameraHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, co
     this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
 }
 
-
 // ----------------------------------------------------------------------
 // Helper method implementations
 // ----------------------------------------------------------------------
@@ -219,6 +218,22 @@ void CameraHandler ::processProtocolBuffer() {
     }
 
     if (headerStart == -1) {
+        
+        // Check for PONG response (only if buffer has enough bytes)
+        if (m_protocolBufferSize >= PONG_LEN) {
+            for (U32 i = 0; i <= m_protocolBufferSize - PONG_LEN; ++i) {
+                if (isPong(&m_protocolBuffer[i], m_protocolBufferSize - i)) {
+                    if (this->m_waiting_for_pong){
+                        this->log_ACTIVITY_HI_PongReceived();
+                        this->m_waiting_for_pong = false;
+                    } else {
+                        this->log_WARNING_HI_BadPongReceived();
+                    }
+                    return;
+                }
+            }
+        }
+        
         // No header found - if buffer is nearly full, discard old data
         // Be aggressive: if buffer is > 50% full and no header, it's probably text responses
         if (m_protocolBufferSize > (PROTOCOL_BUFFER_SIZE / 2)) {
@@ -496,4 +511,21 @@ bool CameraHandler ::isImageStartCommand(const U8* line, U32 length) {
 
     return true;
 }
+
+bool CameraHandler ::isPong(const U8* line, U32 length) {
+    const char* command = "PONG";
+    
+    if (length < PONG_LEN) {
+        return false;
+    }
+    
+    for (U32 i = 0; i < PONG_LEN; ++i) {
+        if (line[i] != static_cast<U8>(command[i])) {
+            return false;
+        }
+    }
+    
+    return true;
+}
+
 }  // namespace Components

FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp

@@ -10,9 +10,6 @@ module Components {
         @ Camera Ping
         sync command PING()
 
-        @ Set Camera Format
-        sync command SET_IMAGE_QUALITY()
-
         @ Send command to camera via PayloadCom
         sync command SEND_COMMAND(cmd: string)
 
@@ -31,6 +28,12 @@ module Components {
         @ Emitted when image transfer completes successfully
         event ImageTransferComplete(path: string, $size: U32) severity activity high format "Image saved: {} ({} bytes)"
 
+        event FailedCommandCurrentlyReceiving() severity warning low format "Cannot send command while image is receiving!"
+
+        event PongReceived() severity activity high format "Ping Received"
+
+        event BadPongReceived() severity warning high format "Ping Received when we did not expect it!"
+
         # Telemetry for debugging image transfer state
         @ Number of bytes received so far in current image transfer
         telemetry BytesReceived: U32

FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp

@@ -58,10 +58,6 @@ class CameraHandler final : public CameraHandlerComponentBase {
     void PING_cmdHandler(FwOpcodeType opCode,  //!< The opcode
                                 U32 cmdSeq            //!< The command sequence number
                                 ) override;
-    
-    void SET_IMAGE_QUALITY_cmdHandler(FwOpcodeType opCode,  //!< The opcode
-                                U32 cmdSeq            //!< The command sequence number
-                                ) override;
 
     // ----------------------------------------------------------------------
     // Helper methods for protocol processing
@@ -95,12 +91,16 @@ class CameraHandler final : public CameraHandlerComponentBase {
     //! Returns true if line is "<IMG_START>"
     bool isImageStartCommand(const U8* line, U32 length);
 
+    bool isPong(const U8* line, U32 length);
+
     // ----------------------------------------------------------------------
     // Member variables
     // ----------------------------------------------------------------------
 
     U8 m_data_file_count = 0;
     bool m_receiving = false; 
+    bool m_waiting_for_pong = false;
+
     U32 m_bytes_received = 0;
     U32 m_file_error_count = 0;  // Track total file errors
     U32 m_images_saved = 0;      // Track total images successfully saved
@@ -123,12 +123,17 @@ class CameraHandler final : public CameraHandlerComponentBase {
     static constexpr U32 SIZE_VALUE_LEN = 4;     // 4-byte little-endian uint32
     static constexpr U32 SIZE_CLOSE_TAG_LEN = 7; // strlen("</SIZE>")
     static constexpr U32 IMG_END_LEN = 9;        // strlen("<IMG_END>")
-    
+    static constexpr U32 PONG_LEN = 4;           // strlen("PONG")
+    static constexpr U32 QUAL_SET_HD = 22;         // strlen("<FRAME_CHANGE_SUCCESS>")
+
+
     // Derived constants
     static constexpr U32 HEADER_SIZE = IMG_START_LEN + SIZE_TAG_LEN + SIZE_VALUE_LEN + SIZE_CLOSE_TAG_LEN;  // 28 bytes
     static constexpr U32 SIZE_TAG_OFFSET = IMG_START_LEN;                    // 11
     static constexpr U32 SIZE_VALUE_OFFSET = IMG_START_LEN + SIZE_TAG_LEN;   // 17
     static constexpr U32 SIZE_CLOSE_TAG_OFFSET = SIZE_VALUE_OFFSET + SIZE_VALUE_LEN;  // 21
+
+
 
     U32 m_expected_size = 0;  // Expected image size from header
     U8 m_lastMilestone = 0;   // Last progress milestone emitted (0, 25, 50, 75)