diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index 0190a9cb..7c290c7d 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -14,3 +14,5 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/PowerMonitor/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ResetManager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/StartupManager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Watchdog") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/PayloadCom/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/CameraHandler/") diff --git a/FprimeZephyrReference/Components/CameraHandler/CMakeLists.txt b/FprimeZephyrReference/Components/CameraHandler/CMakeLists.txt new file mode 100644 index 00000000..b520fa05 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/CMakeLists.txt @@ -0,0 +1,36 @@ +#### +# F Prime CMakeLists.txt: +# +# SOURCES: list of source files (to be compiled) +# AUTOCODER_INPUTS: list of files to be passed to the autocoders +# DEPENDS: list of libraries that this module depends on +# +# More information in the F´ CMake API documentation: +# https://fprime.jpl.nasa.gov/latest/docs/reference/api/cmake/API/ +# +#### + +# Module names are derived from the path from the nearest project/library/framework +# root when not specifically overridden by the developer. i.e. The module defined by +# `Ref/SignalGen/CMakeLists.txt` will be named `Ref_SignalGen`. + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/CameraHandler.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/CameraHandler.cpp" +# DEPENDS +# MyPackage_MyOtherModule +) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/CameraHandler.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/CameraHandlerTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/CameraHandlerTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp new file mode 100644 index 00000000..a296e426 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -0,0 +1,533 @@ +// ====================================================================== +// \title CameraHandler.cpp +// \author moises +// \brief cpp file for CameraHandler component implementation class +// Handles camera protocol processing and image file saving +// ====================================================================== + +#include "Os/File.hpp" +#include "Fw/Types/Assert.hpp" +#include "Fw/Types/BasicTypes.hpp" +#include +#include +#include "FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp" + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +CameraHandler ::CameraHandler(const char* const compName) : CameraHandlerComponentBase(compName) {} + +CameraHandler ::~CameraHandler() { + // Close file if still open + if (m_fileOpen) { + m_file.close(); + m_fileOpen = false; + } +} + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { + // Check if we received data successfully + if (status != Drv::ByteStreamStatus::OP_OK) { + // Log error and abort if receiving + if (m_receiving && m_fileOpen) { + handleFileError(); + } + // NOTE: PayloadCom will handle buffer return, not us + return; + } + + // Check if buffer is valid + if (!buffer.isValid()) { + return; + } + + // Get the data from the buffer (we don't own it, just read it) + const U8* data = buffer.getData(); + U32 dataSize = static_cast(buffer.getSize()); + + // Emit telemetry to track state at entry to handler + this->tlmWrite_BytesReceived(m_bytes_received); + this->tlmWrite_ExpectedSize(m_expected_size); + this->tlmWrite_IsReceiving(m_receiving); + this->tlmWrite_FileOpen(m_fileOpen); + + if (m_receiving && m_fileOpen) { + // Currently receiving image data - write directly to file + + // Calculate how much to write (don't exceed expected size) + U32 remaining = m_expected_size - m_bytes_received; + U32 toWrite = (dataSize < remaining) ? dataSize : remaining; + + // Check if we've received all expected data + if (m_bytes_received >= m_expected_size) { + // Image is complete! + finalizeImageTransfer(); + + // If there's extra data after the image (e.g., or next header), + // push it to protocol buffer for processing + U32 extraBytes = dataSize - toWrite; + if (extraBytes > 0) { + const U8* extraData = data + toWrite; + if (accumulateProtocolData(extraData, extraBytes)) { + processProtocolBuffer(); + } + } + + // Transfer is complete - don't try to write anything more + return; + } + + // Write chunk to file + if (!writeChunkToFile(data, toWrite)) { + // Write failed + this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write failed")); + handleFileError(); + return; + } + + m_bytes_received += toWrite; + + // Emit telemetry after each write + this->tlmWrite_BytesReceived(m_bytes_received); + this->tlmWrite_ExpectedSize(m_expected_size); + + // Emit progress events at 25%, 50%, 75% milestones + if (m_expected_size > 0) { + U8 currentPercent = static_cast((static_cast(m_bytes_received) * 100) / m_expected_size); + + if (currentPercent >= 25 && m_lastMilestone < 25) { + this->log_ACTIVITY_HI_ImageTransferProgress(25, m_bytes_received, m_expected_size); + m_lastMilestone = 25; + } else if (currentPercent >= 50 && m_lastMilestone < 50) { + this->log_ACTIVITY_HI_ImageTransferProgress(50, m_bytes_received, m_expected_size); + m_lastMilestone = 50; + } else if (currentPercent >= 75 && m_lastMilestone < 75) { + this->log_ACTIVITY_HI_ImageTransferProgress(75, m_bytes_received, m_expected_size); + m_lastMilestone = 75; + } + } + + } else { + // Not receiving image - accumulate protocol data + + // If protocol buffer is getting too full (> 90%), clear old data + // This prevents overflow from text responses that aren't image headers + if (m_protocolBufferSize > (PROTOCOL_BUFFER_SIZE * 9 / 10)) { + // Keep only last 32 bytes in case header is split + if (m_protocolBufferSize > 32) { + memmove(m_protocolBuffer, &m_protocolBuffer[m_protocolBufferSize - 32], 32); + m_protocolBufferSize = 32; + } + } + + if (!accumulateProtocolData(data, dataSize)) { + // Protocol buffer overflow - clear old data and keep new + clearProtocolBuffer(); + // Try again with cleared buffer + if (!accumulateProtocolData(data, dataSize)) { + // Still won't fit - just take what we can + U32 canFit = PROTOCOL_BUFFER_SIZE; + memcpy(m_protocolBuffer, data, canFit); + m_protocolBufferSize = canFit; + } + } + + // Process protocol buffer to detect image headers/commands + processProtocolBuffer(); + } + + // NOTE: Do NOT return buffer here - PayloadCom owns the buffer and will return it + // Returning it twice causes buffer management issues +} + +// ---------------------------------------------------------------------- +// Handler implementations for commands +// ---------------------------------------------------------------------- + +void CameraHandler ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + const char* takeImageCmd = "snap"; + SEND_COMMAND_cmdHandler(opCode, cmdSeq, Fw::CmdStringArg(takeImageCmd)); +} + +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 ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) { + // Append newline to command to send to PayloadCom + Fw::CmdStringArg tempCmd = cmd; + tempCmd += "\n"; + Fw::Buffer commandBuffer( + reinterpret_cast(const_cast(tempCmd.toChar())), + tempCmd.length() + ); + + // Send command to PayloadCom (which will forward to UART) + // ByteStreamData ports require buffer and status + this->commandOut_out(0, commandBuffer, Drv::ByteStreamStatus::OP_OK); + + Fw::LogStringArg logCmd(cmd); + this->log_ACTIVITY_HI_CommandSuccess(logCmd); + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + +// ---------------------------------------------------------------------- +// Helper method implementations +// ---------------------------------------------------------------------- + +bool CameraHandler ::accumulateProtocolData(const U8* data, U32 size) { + // Check if we have space for the new data + if (m_protocolBufferSize + size > PROTOCOL_BUFFER_SIZE) { + return false; + } + + // Copy data into protocol buffer + memcpy(&m_protocolBuffer[m_protocolBufferSize], data, size); + m_protocolBufferSize += size; + + return true; +} + +void CameraHandler ::processProtocolBuffer() { + // Protocol: [4-byte little-endian uint32][image data] + + // Search for anywhere in the buffer (not just at position 0) + I32 headerStart = -1; + + // Only search if we have enough bytes for the header marker + if (m_protocolBufferSize >= IMG_START_LEN) { + for (U32 i = 0; i <= m_protocolBufferSize - IMG_START_LEN; ++i) { + if (isImageStartCommand(&m_protocolBuffer[i], m_protocolBufferSize - i)) { + headerStart = static_cast(i); + break; + } + } + } + + 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)) { + // Keep last 16 bytes in case header is split across chunks + if (m_protocolBufferSize > 16) { + memmove(m_protocolBuffer, &m_protocolBuffer[m_protocolBufferSize - 16], 16); + m_protocolBufferSize = 16; + } else { + // Buffer is small enough, just clear it + clearProtocolBuffer(); + } + } + return; + } + + // Found header start! Discard everything before it + if (headerStart > 0) { + U32 remaining = m_protocolBufferSize - static_cast(headerStart); + memmove(m_protocolBuffer, &m_protocolBuffer[headerStart], remaining); + m_protocolBufferSize = remaining; + } + + // Now check if we have the complete header (28 bytes minimum) + if (m_protocolBufferSize < HEADER_SIZE) { + // Not enough data yet, wait for more + return; + } + + // Check if we have the complete header + if (m_protocolBufferSize >= HEADER_SIZE) { + // Check for at the beginning + if (!isImageStartCommand(m_protocolBuffer, m_protocolBufferSize)) { + return; // Not an image header + } + + // Check for tag after + const char* sizeTag = ""; + bool hasSizeTag = true; + + for (U32 i = 0; i < SIZE_TAG_LEN; ++i) { + if (m_protocolBuffer[SIZE_TAG_OFFSET + i] != static_cast(sizeTag[i])) { + hasSizeTag = false; + break; + } + } + + if (!hasSizeTag) { + return; // Invalid header + } + + // Extract 4-byte size (little-endian) + U32 imageSize = 0; + imageSize |= static_cast(m_protocolBuffer[SIZE_VALUE_OFFSET + 0]); + imageSize |= static_cast(m_protocolBuffer[SIZE_VALUE_OFFSET + 1]) << 8; + imageSize |= static_cast(m_protocolBuffer[SIZE_VALUE_OFFSET + 2]) << 16; + imageSize |= static_cast(m_protocolBuffer[SIZE_VALUE_OFFSET + 3]) << 24; + + // Verify tag + const char* closeSizeTag = ""; + bool hasCloseSizeTag = true; + + for (U32 i = 0; i < SIZE_CLOSE_TAG_LEN; ++i) { + if (m_protocolBuffer[SIZE_CLOSE_TAG_OFFSET + i] != static_cast(closeSizeTag[i])) { + hasCloseSizeTag = false; + break; + } + } + + if (!hasCloseSizeTag) { + return; // Invalid header + } + + // Valid header! Open file immediately for streaming + m_receiving = true; + m_bytes_received = 0; + m_expected_size = imageSize; + m_lastMilestone = 0; // Reset milestone tracking for new transfer + + // Generate filename - save to root filesystem + char filename[64]; + // Get parameter for image number + Fw::ParamValid valid = Fw::ParamValid::VALID; + snprintf(filename, sizeof(filename), "/cam%03d_img_%03d.jpg", this->cam_number, this->m_images_saved++); + m_currentFilename = filename; + + // Open file for writing + Os::File::Status status = m_file.open(m_currentFilename.c_str(), Os::File::OPEN_WRITE); + + if (status != Os::File::OP_OK) { + // Failed to open file + this->log_WARNING_HI_CommandError(Fw::LogStringArg("Failed to open file")); + m_receiving = false; + m_expected_size = 0; + clearProtocolBuffer(); + return; + } + + m_fileOpen = true; + + // Log transfer started event + this->log_ACTIVITY_HI_ImageTransferStarted(imageSize); + + // Emit telemetry after opening file + this->tlmWrite_BytesReceived(m_bytes_received); + this->tlmWrite_ExpectedSize(m_expected_size); + this->tlmWrite_IsReceiving(m_receiving); + this->tlmWrite_FileOpen(m_fileOpen); + + // NOTE: PayloadCom sends ACK automatically after forwarding data + // No need to send ACK here - that's handled by the communication layer + + // Remove header from protocol buffer + U32 remainingSize = m_protocolBufferSize - HEADER_SIZE; + if (remainingSize > 0) { + memmove(m_protocolBuffer, + &m_protocolBuffer[HEADER_SIZE], + remainingSize); + } + m_protocolBufferSize = remainingSize; + + // Write any remaining data (image data) directly to file + // NOTE: This should be empty since camera waits for ACK before sending data + if (m_protocolBufferSize > 0) { + U32 toWrite = (m_protocolBufferSize < m_expected_size) ? m_protocolBufferSize : m_expected_size; + + if (writeChunkToFile(m_protocolBuffer, toWrite)) { + m_bytes_received += toWrite; + + // Check if complete already + if (m_bytes_received >= m_expected_size) { + finalizeImageTransfer(); + } + } else { + handleFileError(); + } + + clearProtocolBuffer(); + } + } +} + +void CameraHandler ::clearProtocolBuffer() { + m_protocolBufferSize = 0; + memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); +} + +bool CameraHandler ::writeChunkToFile(const U8* data, U32 size) { + if (!m_fileOpen || size == 0) { + return false; + } + + // Write data to file, handling partial writes + U32 totalWritten = 0; + const U8* ptr = data; + + while (totalWritten < size) { + FwSizeType toWrite = static_cast(size - totalWritten); + Os::File::Status status = m_file.write(ptr, toWrite, Os::File::WaitType::WAIT); + + if (status != Os::File::OP_OK) { + return false; + } + + // toWrite now contains the actual bytes written + totalWritten += static_cast(toWrite); + ptr += toWrite; + } + + // Log bytes written + // this->log_ACTIVITY_LO_ChunkWritten(totalWritten); + + return true; +} + +void CameraHandler ::finalizeImageTransfer() { + if (!m_fileOpen) { + return; + } + + // Close the file + m_file.close(); + m_fileOpen = false; + + // Increment success counter + m_images_saved++; + + // Log transfer complete event with path and size + Fw::LogStringArg pathArg(m_currentFilename.c_str()); + this->log_ACTIVITY_HI_ImageTransferComplete(pathArg, m_bytes_received); + + // NOTE: PayloadCom sends ACK automatically - no need to send here + + // Reset state + m_receiving = false; + m_bytes_received = 0; + m_expected_size = 0; + m_lastMilestone = 0; + + // Emit telemetry after finalizing + this->tlmWrite_BytesReceived(m_bytes_received); + this->tlmWrite_ExpectedSize(m_expected_size); + this->tlmWrite_IsReceiving(m_receiving); + this->tlmWrite_FileOpen(m_fileOpen); + this->tlmWrite_ImagesSaved(m_images_saved); +} + +void CameraHandler ::handleFileError() { + // Close file if open + if (m_fileOpen) { + m_file.close(); + m_fileOpen = false; + } + + // Increment error counter + m_file_error_count++; + + // Log error + this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write error")); + + // Reset state + m_receiving = false; + m_bytes_received = 0; + m_expected_size = 0; + m_lastMilestone = 0; + clearProtocolBuffer(); + + // Emit telemetry after error handling + this->tlmWrite_BytesReceived(m_bytes_received); + this->tlmWrite_ExpectedSize(m_expected_size); + this->tlmWrite_IsReceiving(m_receiving); + this->tlmWrite_FileOpen(m_fileOpen); + this->tlmWrite_FileErrorCount(m_file_error_count); +} + +I32 CameraHandler ::findImageEndMarker(const U8* data, U32 size) { + // Looking for "\n" or "" + const char* marker = ""; + + if (size < IMG_END_LEN) { + return -1; + } + + // Search for the marker + for (U32 i = 0; i <= size - IMG_END_LEN; ++i) { + bool found = true; + for (U32 j = 0; j < IMG_END_LEN; ++j) { + if (data[i + j] != static_cast(marker[j])) { + found = false; + break; + } + } + if (found) { + // Found marker at position i + // If preceded by newline, back up to before newline + if (i > 0 && data[i - 1] == '\n') { + return static_cast(i - 1); + } + return static_cast(i); + } + } + + return -1; // Not found +} + +bool CameraHandler ::isImageStartCommand(const U8* line, U32 length) { + const char* command = ""; + + if (length < IMG_START_LEN) { + return false; + } + + for (U32 i = 0; i < IMG_START_LEN; ++i) { + if (line[i] != static_cast(command[i])) { + return false; + } + } + + 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(command[i])) { + return false; + } + } + + return true; +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp new file mode 100644 index 00000000..abe3a63d --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -0,0 +1,113 @@ +module Components { + @ Active component that handles camera-specific payload protocol processing and file saving + @ Receives data from PayloadCom, parses image protocol, saves files + passive component CameraHandler { + + # Commands + @ Type in "snap" to capture an image + sync command TAKE_IMAGE() + + @ Camera Ping + sync command PING() + + @ Send command to camera via PayloadCom + sync command SEND_COMMAND(cmd: string) + + # Events for command handling + event CommandError(cmd: string) severity warning high format "Failed to send {} command" + + event CommandSuccess(cmd: string) severity activity high format "Command {} sent successfully" + + # Events for image transfer (clean, minimal output) + @ Emitted when image transfer begins + event ImageTransferStarted($size: U32) severity activity high format "Image transfer started, expecting {} bytes" + + @ Emitted at 25%, 50%, 75% progress milestones + event ImageTransferProgress(percent: U8, received: U32, expected: U32) severity activity high format "Image transfer {}% complete ({}/{} bytes)" + + @ 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 + + @ Expected total size of image being received + telemetry ExpectedSize: U32 + + @ Whether currently receiving image data + telemetry IsReceiving: bool + + @ Whether file is currently open for writing + telemetry FileOpen: bool + + @ Total number of file errors encountered + telemetry FileErrorCount: U32 + + @ Total number of images successfully saved + telemetry ImagesSaved: U32 + + # Ports + @ Sends command to PayloadCom to be forwarded over UART + output port commandOut: Drv.ByteStreamData + + @ Receives data from PayloadCom, handles image protocol parsing and file saving + sync input port dataIn: Drv.ByteStreamData + + ############################################################################## + #### Uncomment the following examples to start customizing your component #### + ############################################################################## + + # @ Example async command + # async command COMMAND_NAME(param_name: U32) + + # @ Example telemetry counter + # telemetry ExampleCounter: U64 + + # @ Example event + # event ExampleStateEvent(example_state: Fw.On) severity activity high id 0 format "State set to {}" + + # @ Example port: receiving calls from the rate group + # sync input port run: Svc.Sched + + # @ Example parameter + # param PARAMETER_NAME: U32 + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + @ Port for sending command registrations + command reg port cmdRegOut + + @ Port for receiving commands + command recv port cmdIn + + @ Port for sending command responses + command resp port cmdResponseOut + + @ Port for sending textual representation of events + text event port logTextOut + + @ Port for sending events to downlink + event port logOut + + @ Port for sending telemetry channels to downlink + telemetry port tlmOut + + @ Port to return the value of a parameter + param get port prmGetOut + + @Port to set the value of a parameter + param set port prmSetOut + + } +} diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp new file mode 100644 index 00000000..59953715 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -0,0 +1,149 @@ +// ====================================================================== +// \title CameraHandler.hpp +// \author moises +// \brief hpp file for CameraHandler component implementation class +// Handles camera protocol processing and image file saving +// ====================================================================== + +#ifndef Components_CameraHandler_HPP +#define Components_CameraHandler_HPP + +#include +#include +#include "FprimeZephyrReference/Components/CameraHandler/CameraHandlerComponentAc.hpp" +#include "Os/File.hpp" + +namespace Components { + +class CameraHandler final : public CameraHandlerComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct CameraHandler object + CameraHandler(const char* const compName //!< The component name + ); + + //! Destroy CameraHandler object + ~CameraHandler(); + + void configure(U32 cam_num) { + this->cam_number = cam_num; + } + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for dataIn + //! Receives data from PayloadCom, handles image protocol parsing and file saving + void dataIn_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& buffer, + const Drv::ByteStreamStatus& status) override; + + private: + // ---------------------------------------------------------------------- + // Handler implementations for commands + // ---------------------------------------------------------------------- + + //! Handler implementation for command TAKE_IMAGE + //! Type in "snap" to capture an image + void TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; + + //! Handler implementation for command SEND_COMMAND + void SEND_COMMAND_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq, //!< The command sequence number + const Fw::CmdStringArg& cmd) override; + + void PING_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; + + // ---------------------------------------------------------------------- + // Helper methods for protocol processing + // ---------------------------------------------------------------------- + + //! Accumulate protocol data (headers, commands) + //! Returns true if data was successfully accumulated, false on overflow + bool accumulateProtocolData(const U8* data, U32 size); + + //! Process protocol buffer to detect commands/image headers + void processProtocolBuffer(); + + //! Clear the protocol buffer + void clearProtocolBuffer(); + + //! Write data chunk directly to open file + //! Returns true on success + bool writeChunkToFile(const U8* data, U32 size); + + //! Close file and finalize image transfer + void finalizeImageTransfer(); + + //! Handle file write error + void handleFileError(); + + //! Check if buffer contains image end marker + //! Returns position of marker start, or -1 if not found + I32 findImageEndMarker(const U8* data, U32 size); + + //! Parse line for image start command + //! Returns true if line is "" + 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 + U32 cam_number = 0; // Camera number for filename generation + + U8 m_lineBuffer[128]; + size_t m_lineIndex = 0; + Os::File m_file; + std::string m_currentFilename; + bool m_fileOpen = false; // Track if file is currently open for writing + + // Small protocol buffer for commands/headers (static allocation) + static constexpr U32 PROTOCOL_BUFFER_SIZE = 128; // Just enough for header + U8 m_protocolBuffer[PROTOCOL_BUFFER_SIZE]; + U32 m_protocolBufferSize = 0; + + // Protocol constants for image transfer + // Protocol: [4-byte uint32][image data] + static constexpr U32 IMG_START_LEN = 11; // strlen("") + static constexpr U32 SIZE_TAG_LEN = 6; // strlen("") + static constexpr U32 SIZE_VALUE_LEN = 4; // 4-byte little-endian uint32 + static constexpr U32 SIZE_CLOSE_TAG_LEN = 7; // strlen("") + static constexpr U32 IMG_END_LEN = 9; // strlen("") + static constexpr U32 PONG_LEN = 4; // strlen("PONG") + static constexpr U32 QUAL_SET_HD = 22; // strlen("") + + + // 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) +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md new file mode 100644 index 00000000..90c42299 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md @@ -0,0 +1,60 @@ +# Components::CameraHandler + +Passive component that handles camera specific payload capabilities. + - Taking Images + - Pinging + +## Usage Examples +The camera handler can be commanded to take an image, after which it will forward a command to the PayloadCom component. It will then read in data from the PayloadCom until the image has finished sending. + +### Typical Usage +Prior to taking a picture, the payload power loadswitch must be activated. Then "PING" the camera with the ping command. If the PING command returns successfully, then the camera is ready to take an image. + +## Port Descriptions +| Name | Description | +|------------|---------------------------------------------------------| +| commandOut | Command to forward to the PayloadCom component | +| dataIn | Data received from the PayloadCom component | + +## Component States +Add component states in the chart below +| Name | Description | +|-------------|-------------------------------------------------------| +| m_receiving | True when the camera is currently receiving image data | + +## Commands +| Name | Description | +|--------------|-------------------------------------------------------| +| PING | Send a ping to the camera – wait for a response | +| TAKE_IMAGE | Send "snap" command to the payload com component | +| SEND_COMMAND | Send a user-specified command to the payload com component | + +## Events +| Name | Description | +|---|---| +|---|---| + +## Telemetry +| Name | Description | +|---|---| +|---|---| + +## Unit Tests +Add unit test descriptions in the chart below +| Name | Description | Output | Coverage | +|---|---|---|---| +|---|---|---|---| + +## Requirements +Add requirements in the chart below +| Name | Description | Validation | +| -----|-------------|------------| +| CameraHandler-001 | The CameraHandler has a command to take an image. | Manual Test | +| CameraHandler-002 | The CameraHandler has a command to "ping" the camera. | Manual Test | +| CameraHandler-003 | The Camera Handler forwards the commands to the PayloadCom Component. | Manual Test | +| CameraHandler-004 | The Camera Handler receives all image data bytes and saves them to a new file. | Manual Test | + +## Change Log +| Date | Description | +|---|---| +|---| Initial Draft | diff --git a/FprimeZephyrReference/Components/PayloadCom/CMakeLists.txt b/FprimeZephyrReference/Components/PayloadCom/CMakeLists.txt new file mode 100644 index 00000000..77764c68 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadCom/CMakeLists.txt @@ -0,0 +1,36 @@ +#### +# F Prime CMakeLists.txt: +# +# SOURCES: list of source files (to be compiled) +# AUTOCODER_INPUTS: list of files to be passed to the autocoders +# DEPENDS: list of libraries that this module depends on +# +# More information in the F´ CMake API documentation: +# https://fprime.jpl.nasa.gov/latest/docs/reference/api/cmake/API/ +# +#### + +# Module names are derived from the path from the nearest project/library/framework +# root when not specifically overridden by the developer. i.e. The module defined by +# `Ref/SignalGen/CMakeLists.txt` will be named `Ref_SignalGen`. + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/PayloadCom.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/PayloadCom.cpp" +# DEPENDS +# MyPackage_MyOtherModule +) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/PayloadCom.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/PayloadComTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/PayloadComTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp new file mode 100644 index 00000000..74be6bec --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp @@ -0,0 +1,93 @@ +// ====================================================================== +// \title PayloadCom.cpp +// \author robertpendergrast, moisesmata +// \brief cpp file for PayloadCom component implementation class +// ====================================================================== +#include "FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp" +#include "Fw/Types/BasicTypes.hpp" +#include + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +PayloadCom ::PayloadCom(const char* const compName) + : PayloadComComponentBase(compName) {} + +PayloadCom ::~PayloadCom() {} + + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void PayloadCom ::uartDataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { + // this->log_ACTIVITY_LO_UartReceived(); + + // Check if we received data successfully + if (status != Drv::ByteStreamStatus::OP_OK) { + // Must return buffer even on error to prevent leak + if (buffer.isValid()) { + this->bufferReturn_out(0, buffer); + } + return; + } + + // Forward data to specific payload handler for protocol processing + this->uartDataOut_out(0, buffer, status); + + // Send ACK to acknowledge receipt + sendAck(); + + // CRITICAL: Return buffer to driver so it can deallocate to BufferManager + // This matches the ComStub pattern: driver allocates, handler processes, handler returns + this->bufferReturn_out(0, buffer); +} + +void PayloadCom ::commandIn_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { + // Log received command for debugging + if (buffer.isValid()) { + U32 size = static_cast(buffer.getSize()); + Fw::LogStringArg logStr("Forwarding command"); + this->log_ACTIVITY_HI_CommandForwardSuccess(logStr); + } + + // Forward command from CameraHandler to UART + // uartForward is ByteStreamSend which returns status + Drv::ByteStreamStatus sendStatus = this->uartForward_out(0, buffer); + + // Log if send failed (optional) + if (sendStatus != Drv::ByteStreamStatus::OP_OK) { + Fw::LogStringArg logStr("command"); + this->log_WARNING_HI_CommandForwardError(logStr); + } else { + Fw::LogStringArg logStr("command"); + this->log_ACTIVITY_HI_CommandForwardSuccess(logStr); + } +} + +// ---------------------------------------------------------------------- +// Helper method implementations +// ---------------------------------------------------------------------- + +void PayloadCom ::sendAck(){ + // Send an acknowledgment over UART + const char* ackMsg = "\n"; + Fw::Buffer ackBuffer( + reinterpret_cast(const_cast(ackMsg)), + strlen(ackMsg) + ); + // uartForward is ByteStreamSend which returns status + Drv::ByteStreamStatus sendStatus = this->uartForward_out(0, ackBuffer); + + if (sendStatus == Drv::ByteStreamStatus::OP_OK) { + // this->log_ACTIVITY_LO_AckSent(); + } else { + Fw::LogStringArg logStr("ACK"); + this->log_WARNING_HI_CommandForwardError(logStr); + } +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp new file mode 100644 index 00000000..de69f0c9 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp @@ -0,0 +1,79 @@ +module Components { + @ Barebones UART communication layer for payload (Nicla Vision camera) + @ Handles UART forwarding and ACK handshake, protocol processing done by specific payload handler + active component PayloadCom { + + event CommandForwardError(cmd: string) severity warning high format "Failed to send {} command over UART" + + event CommandForwardSuccess(cmd: string) severity activity high format "Command {} sent successfully" + + event UartReceived() severity activity low format "Received UART data" + + event AckSent() severity activity low format "ACK sent to payload" + + @ Receives the desired command to forward through the payload UART + sync input port commandIn: Drv.ByteStreamData + + @ Receives data from the UART, forwards to handler and sends ACK + async input port uartDataIn: Drv.ByteStreamData + + @ Sends data to the UART (forwards commands and acknowledgement signals) + output port uartForward: Drv.ByteStreamSend + + @ Return RX buffers to UART driver (driver will deallocate to BufferManager) + output port bufferReturn: Fw.BufferSend + + @ Sends data that is received by the uartDataIn port to the desired payload handler component + output port uartDataOut: Drv.ByteStreamData + + ############################################################################## + #### Uncomment the following examples to start customizing your component #### + ############################################################################## + + # @ Example async command + # async command COMMAND_NAME(param_name: U32) + + # @ Example telemetry counter + # telemetry ExampleCounter: U64 + + # @ Example event + # event ExampleStateEvent(example_state: Fw.On) severity activity high id 0 format "State set to {}" + + # @ Example port: receiving calls from the rate group + # sync input port run: Svc.Sched + + # @ Example parameter + # param PARAMETER_NAME: U32 + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + @ Port for sending command registrations + command reg port cmdRegOut + + @ Port for receiving commands + command recv port cmdIn + + @ Port for sending command responses + command resp port cmdResponseOut + + @ Port for sending textual representation of events + text event port logTextOut + + @ Port for sending events to downlink + event port logOut + + @ Port for sending telemetry channels to downlink + telemetry port tlmOut + + @ Port to return the value of a parameter + param get port prmGetOut + + @Port to set the value of a parameter + param set port prmSetOut + + } +} diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp new file mode 100644 index 00000000..9daa2bc2 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp @@ -0,0 +1,55 @@ +// ====================================================================== +// \title PayloadCom.hpp +// \author robertpendergrast, moisesmata +// \brief hpp file for PayloadCom component implementation class +// ====================================================================== + +#ifndef FprimeZephyrReference_PayloadCom_HPP +#define FprimeZephyrReference_PayloadCom_HPP + +#include "FprimeZephyrReference/Components/PayloadCom/PayloadComComponentAc.hpp" + +namespace Components { + +class PayloadCom final : public PayloadComComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct PayloadCom object + PayloadCom(const char* const compName //!< The component name + ); + + //! Destroy PayloadCom object + ~PayloadCom(); + + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for uartDataIn port + //! Forwards data to CameraHandler and sends ACK + void uartDataIn_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& buffer, + const Drv::ByteStreamStatus& status); + + //! Handler implementation for commandIn port + //! Forwards command to UART + void commandIn_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& buffer, + const Drv::ByteStreamStatus& status); + + // ---------------------------------------------------------------------- + // Helper methods + // ---------------------------------------------------------------------- + + //! Send acknowledgment over UART + void sendAck(); +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/Components/PayloadCom/docs/BUFFER_USAGE.md b/FprimeZephyrReference/Components/PayloadCom/docs/BUFFER_USAGE.md new file mode 100644 index 00000000..9a1dfe55 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadCom/docs/BUFFER_USAGE.md @@ -0,0 +1,249 @@ +# PayloadCom Buffer Strategy + +## Overview + +The PayloadCom uses a **two-buffer strategy** to efficiently handle both protocol/header data and large image data: + +1. **Small static buffer** (2 KB) for protocol headers/commands +2. **Large dynamic buffer** (256 KB) from BufferManager for image data + +## Why This Approach? + +### Problem with Static Buffers for Images +- **Memory waste**: 100s of KB allocated permanently even when not receiving +- **Stack overflow**: Embedded systems have limited stack/static memory +- **Component bloat**: Every component instance would have huge buffer + +### Solution: BufferManager +- Allocates memory **only when needed** +- Returns memory when done +- Memory pooling for efficiency +- Multiple components can share memory pool + +## Architecture + +``` +UART Driver (64 byte chunks) + ↓ + PayloadCom + ↓ + ┌─────┴─────┐ + ↓ ↓ +Protocol Image +Buffer Buffer +(2 KB) (256 KB) +static dynamic +``` + +## Buffer Details + +### Protocol Buffer +- **Size**: 2048 bytes (static allocation) +- **Purpose**: Parse commands, headers, metadata +- **Lifecycle**: Always present +- **Location**: `m_protocolBuffer[PROTOCOL_BUFFER_SIZE]` + +### Image Buffer +- **Size**: 256 KB (dynamic allocation) +- **Purpose**: Accumulate complete image data +- **Lifecycle**: Allocated when receiving, deallocated when done +- **Location**: `m_imageBuffer` (Fw::Buffer from BufferManager) + +## Data Flow + +### 1. Idle State (No Image) +``` +Incoming data → Protocol Buffer → Parse for commands +``` + +### 2. Image Header Detected +``` +Parse "...bytes..." → Extract image size (m_expected_size) + → allocateImageBuffer() + → m_receiving = true + → Start accumulating +``` + +### 3. Receiving Image +``` +Incoming data → Image Buffer (accumulate) → Check if m_imageBufferUsed >= m_expected_size +``` + +### 4. Image Complete +``` +Received m_expected_size bytes → processCompleteImage() → Write to file + → deallocateImageBuffer() + → m_receiving = false +``` + +## Camera Protocol + +The Nicla Vision camera sends images using this protocol: + +``` +[4-byte little-endian uint32][raw JPEG data in 512-byte chunks] +``` + +Example: +``` +\x00\xC8\x00\x00ÿØÿà...JFIF...image data...ÿÙ + ^^^^^^^^^^ = 51200 bytes (little-endian) +``` + +**Key Features:** +- **No newlines** - All data is sent continuously +- **Binary size** - 4-byte little-endian uint32 embedded in header +- **Deterministic** - Exact image size known before data arrives + +## Key Functions + +### Buffer Allocation +```cpp +bool allocateImageBuffer() +``` +- Requests 256 KB buffer from BufferManager +- Returns true on success, false on failure +- Logs event if allocation fails + +### Buffer Deallocation +```cpp +void deallocateImageBuffer() +``` +- Returns buffer to BufferManager +- Called automatically when image complete or on error + +### Data Accumulation +```cpp +bool accumulateProtocolData(const U8* data, U32 size) // Small chunks +bool accumulateImageData(const U8* data, U32 size) // Large chunks +``` + +## Configuration + +### BufferManager Setup (instances.fpp) +```fpp +instance payloadBufferManager: Svc.BufferManager { + // 256 KB buffers, 2 buffers = 512 KB total + bins[0].bufferSize = 256 * 1024 + bins[0].numBuffers = 2 +} +``` + +### Topology Connections (topology.fpp) +```fpp +payload.allocate -> payloadBufferManager.bufferGetCallee +payload.deallocate -> payloadBufferManager.bufferSendIn +``` + +## Memory Usage + +### Static (Always Allocated) +- Protocol buffer: 2 KB +- Component overhead: ~1 KB +- **Total: ~3 KB** + +### Dynamic (Only When Receiving) +- Image buffer: 256 KB (when allocated) +- **Total: 0-256 KB** + +### Pool Configuration +- 2 buffers of 256 KB = **512 KB total pool** +- Allows receiving 2 images simultaneously or buffering one while processing another + +## Customization + +### To Change Buffer Sizes + +1. **Protocol buffer** (PayloadCom.hpp): +```cpp +static constexpr U32 PROTOCOL_BUFFER_SIZE = 2048; // Change this +``` + +2. **Image buffer** (PayloadCom.hpp): +```cpp +static constexpr U32 IMAGE_BUFFER_SIZE = 256 * 1024; // Change this +``` + +3. **BufferManager pool** (instances.fpp): +```cpp +bins[0].bufferSize = 256 * 1024; // Must match IMAGE_BUFFER_SIZE +bins[0].numBuffers = 2; // Number of buffers in pool +``` + +### To Add Different Buffer Sizes + +You can configure multiple buffer bins in the BufferManager: +```cpp +bins[0].bufferSize = 256 * 1024; // 256 KB for large images +bins[0].numBuffers = 2; +bins[1].bufferSize = 64 * 1024; // 64 KB for small images +bins[1].numBuffers = 4; +``` + +BufferManager will allocate the smallest buffer that fits the request. + +## Implementation Details + +### Protocol Parsing (Implemented) + +The `processProtocolBuffer()` function: +1. Waits for minimum 28 bytes (header size) +2. Validates `` tag (11 bytes) +3. Validates `` tag (6 bytes) +4. Extracts 4-byte little-endian size +5. Validates `` tag (7 bytes) +6. Calls `allocateImageBuffer()` and sets `m_receiving = true`, `m_expected_size = imageSize` +7. Generates filename: `/mnt/data/img_NNN.jpg` + +### Image Reception (Implemented) + +The `in_port_handler()` function: +- When `m_receiving == true`, accumulates data into image buffer +- After each chunk, checks if `m_imageBufferUsed >= m_expected_size` +- When complete, trims to exact size and calls `processCompleteImage()` + +### Size-Based Completion + +The camera sends the exact image size in the header, so we know precisely when we've received all data: + +```cpp +if (m_expected_size > 0 && m_imageBufferUsed >= m_expected_size) { + m_imageBufferUsed = m_expected_size; // Trim any extra (e.g., ) + processCompleteImage(); +} +``` + +No need to search for end markers in every chunk! + +## Error Handling + +### Allocation Failure +- Logs `BufferAllocationFailed` event +- Continues processing protocol buffer +- Can retry later if needed + +### Buffer Overflow +- Image buffer overflow: Logs `ImageDataOverflow` event, deallocates buffer +- Protocol buffer overflow: Clears buffer and restarts + +### Component Destruction +- Destructor automatically deallocates any active image buffer +- Prevents memory leaks + +## Performance Notes + +- **Allocation overhead**: ~microseconds (one-time per image) +- **Copy overhead**: memcpy for each 64-byte UART chunk (~100 cycles) +- **Memory efficiency**: Only allocates when receiving, 256x better than static +- **Latency**: No impact on UART receive rate (64 bytes @ 10Hz = 640 bytes/sec) + +## Testing Checklist + +- [ ] Receive small image (< 256 KB) +- [ ] Receive large image (> 256 KB) - should fail gracefully +- [ ] Receive multiple images sequentially +- [ ] Handle allocation failure (simulate by allocating 2 buffers) +- [ ] Handle protocol buffer overflow +- [ ] Handle image buffer overflow +- [ ] Component cleanup (destructor) + diff --git a/FprimeZephyrReference/Components/PayloadCom/docs/USAGE.md b/FprimeZephyrReference/Components/PayloadCom/docs/USAGE.md new file mode 100644 index 00000000..3bd9bcfb --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadCom/docs/USAGE.md @@ -0,0 +1,319 @@ +# PayloadCom Usage Guide + +## Overview + +The PayloadCom receives images from the Nicla Vision camera over UART and saves them to the filesystem. + +## Camera Commands + +### Take a Snapshot + +Send this command to the camera over UART: +``` +snap +``` + +The camera will: +1. Take a photo +2. Save it locally as `images/img_NNNN.jpg` +3. Send it back over UART with the protocol: + ``` + [4-byte size][JPEG data] + ``` + +## F' Commands + +### Send Command to Camera + +Use the F' command interface to send commands to the camera: + +``` +SEND_COMMAND("snap") +``` + +This forwards the command over UART to the camera. + +## Expected Behavior + +### Successful Image Reception + +1. **Command Sent** + - Event: `CommandSuccess` - "Command snap sent successfully" + +2. **Image Start** + - PayloadCom receives `...bytes...` + - Parses image size from 4-byte little-endian value + - Allocates 256 KB buffer from BufferManager + - Event: `ImageHeaderReceived` - "Received image header" + +3. **Image Data** + - Receives 512-byte chunks from camera + - Accumulates in image buffer + - Event: `UartReceived` - "Received UART data" (multiple times) + +4. **Image Complete** + - Receives all expected bytes (size from header) + - Writes image to `/mnt/data/img_NNN.jpg` + - Event: `DataReceived` - "Stored NNNNN bytes of payload data to /mnt/data/img_NNN.jpg" + - Returns buffer to BufferManager + - Note: Camera also sends `` but it's trimmed off + +### Image Filenames + +Images are saved with sequential filenames: +- `/mnt/data/img_000.jpg` +- `/mnt/data/img_001.jpg` +- `/mnt/data/img_002.jpg` +- etc. + +Counter increments each time a valid `...` header is received. + +## Error Cases + +### Buffer Allocation Failed + +**Event:** `BufferAllocationFailed` - "Failed to allocate buffer of size 262144" + +**Cause:** +- BufferManager pool exhausted (both 256 KB buffers in use) +- Previous image reception didn't complete/cleanup + +**Recovery:** +- Wait for current image to complete +- Retry command + +### Image Data Overflow + +**Event:** `ImageDataOverflow` - "Image data overflow - buffer full" + +**Cause:** +- Image larger than 256 KB +- Camera sent more data than buffer can hold + +**Recovery:** +- Increase `IMAGE_BUFFER_SIZE` in PayloadCom.hpp +- Update BufferManager configuration in instances.fpp + +### Command Error + +**Event:** `CommandError` - "Failed to send snap command over UART" + +**Cause:** +- UART driver not ready +- UART send failed + +**Recovery:** +- Check UART connection +- Retry command + +## Monitoring + +### Key Telemetry (from BufferManager) + +Check these telemetry points to monitor buffer usage: + +- `payloadBufferManager.CurrBuffs` - Currently allocated buffers (0-2) +- `payloadBufferManager.TotalBuffs` - Total buffers available (2) +- `payloadBufferManager.HiBuffs` - High water mark +- `payloadBufferManager.NoBuffs` - Count of allocation failures +- `payloadBufferManager.EmptyBuffs` - Count of empty buffer returns + +### Expected Values During Operation + +**Idle:** +``` +CurrBuffs = 0 +TotalBuffs = 2 +``` + +**Receiving Image:** +``` +CurrBuffs = 1 +TotalBuffs = 2 +``` + +**If NoBuffs > 0:** +- Indicates allocation failures occurred +- Check if images are completing properly +- May need to increase buffer count + +## Protocol Details + +### UART Settings + +- Baud rate: 115200 +- Data bits: 8 +- Stop bits: 1 +- Parity: None + +### Camera → Flight Software Protocol + +``` +Command from FS: "snap\n" +Camera response: "[4-byte little-endian uint32]" + [JPEG file bytes - exact size from header] + "" +``` + +**Header format:** +- `` - 11 bytes ASCII +- `` - 6 bytes ASCII +- Size value - 4 bytes binary (little-endian uint32) +- `` - 7 bytes ASCII +- **Total:** 28 bytes + +**Example header for 51200 byte image:** +``` +\x00\xC8\x00\x00 +``` + +### Timing + +- Command → Image start: ~2-3 seconds (camera capture time) +- Image transmission: depends on size + - At 115200 baud ≈ 11.5 KB/s + - 50 KB image ≈ 4-5 seconds + - 200 KB image ≈ 17-18 seconds + +## Testing + +### Basic Test + +1. Ensure camera is powered and UART connected +2. Send command: `SEND_COMMAND("snap")` +3. Monitor events for successful completion +4. Check file exists: `/mnt/data/img_000.jpg` +5. Verify file size is reasonable (typically 30-100 KB for VGA JPEG) + +### Stress Test + +Send multiple consecutive `snap` commands: +``` +SEND_COMMAND("snap") +# Wait for DataReceived event +SEND_COMMAND("snap") +# Wait for DataReceived event +SEND_COMMAND("snap") +# etc. +``` + +Should handle at least 10+ images sequentially without errors. + +### Buffer Exhaustion Test + +Send 3 `snap` commands rapidly (don't wait): +``` +SEND_COMMAND("snap") +SEND_COMMAND("snap") +SEND_COMMAND("snap") # This should fail with BufferAllocationFailed +``` + +First two should succeed, third should fail gracefully. + +## Filesystem Requirements + +### Mount Point + +The handler expects `/mnt/data/` to be a writable filesystem. + +If using a different mount point, update in `processProtocolBuffer()`: +```cpp +snprintf(filename, sizeof(filename), "/your/path/img_%03d.jpg", m_data_file_count++); +``` + +### Space Requirements + +Each VGA JPEG is typically 30-100 KB. + +With 2 MB filesystem, you can store ~20-60 images. + +Monitor filesystem usage with the `fsSpace` component. + +## Troubleshooting + +### No Images Received + +1. **Check UART connection** + - Verify peripheralUartDriver is configured + - Check baud rate matches camera (115200) + +2. **Check camera is responding** + - Send any command and check for `UartReceived` events + - Verify camera power + +3. **Check events** + - Should see `CommandSuccess` when command sent + - Should see `ImageHeaderReceived` when camera responds + +### Images Corrupted + +1. **Check for buffer overflow** + - Look for `ImageDataOverflow` events + - Increase buffer size if needed + +2. **Check UART errors** + - Verify no data corruption on UART + - Check for timing issues + +3. **Verify complete reception** + - Image size in `DataReceived` event should match expected + - Compare with camera's saved file size + +### Images Not Saving + +1. **Check filesystem mounted** + - Verify `/mnt/data/` exists and is writable + +2. **Check disk space** + - Monitor `fsSpace` telemetry + +3. **Check file operations** + - Add error events for file open/write failures + - Check file permissions + +## Advanced Configuration + +### Changing Buffer Pool Size + +To support larger images or more simultaneous captures, edit `instances.fpp`: + +```fpp +// Support 4 images of 512 KB each = 2 MB pool +bins[0].bufferSize = 512 * 1024; +bins[0].numBuffers = 4; +``` + +Must also update `PayloadCom.hpp`: +```cpp +static constexpr U32 IMAGE_BUFFER_SIZE = 512 * 1024; +``` + +### Verifying Image Integrity + +Since the protocol now includes size, you can verify completeness: + +```cpp +// In processCompleteImage() +if (m_imageBufferUsed != m_expected_size) { + this->log_WARNING_HI_ImageSizeMismatch(m_expected_size, m_imageBufferUsed); +} +``` + +This catches truncation or corruption during transmission. + +### Multiple Image Formats + +To support different image sizes/formats, configure multiple buffer bins: + +```fpp +// Small images (thumbnail) +bins[0].bufferSize = 32 * 1024; +bins[0].numBuffers = 4; + +// Large images (full resolution) +bins[1].bufferSize = 256 * 1024; +bins[1].numBuffers = 2; +``` + +BufferManager will automatically select the appropriate bin. + diff --git a/FprimeZephyrReference/Components/PayloadCom/docs/sdd.md b/FprimeZephyrReference/Components/PayloadCom/docs/sdd.md new file mode 100644 index 00000000..6cca5111 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadCom/docs/sdd.md @@ -0,0 +1,66 @@ +# FprimeZephyrReference::CameraManager + +Manager for Nicla Vision + +## Usage Examples +Add usage examples here + +### Diagrams +Add diagrams here + +### Typical Usage +And the typical usage of the component here + +## Class Diagram +Add a class diagram here + +## Port Descriptions +| Name | Description | +|---|---| +|---|---| + +## Component States +Add component states in the chart below +| Name | Description | +|---|---| +|---|---| + +## Sequence Diagrams +Add sequence diagrams here + +## Parameters +| Name | Description | +|---|---| +|---|---| + +## Commands +| Name | Description | +|---|---| +|---|---| + +## Events +| Name | Description | +|---|---| +|---|---| + +## Telemetry +| Name | Description | +|---|---| +|---|---| + +## Unit Tests +Add unit test descriptions in the chart below +| Name | Description | Output | Coverage | +|---|---|---|---| +|---|---|---|---| + +## Requirements +Add requirements in the chart below +| Name | Description | Validation | +|---|---|---| +|---|---|---| + +## Change Log +| Date | Description | +|---|---| +|---| Initial Draft | \ No newline at end of file diff --git a/FprimeZephyrReference/ReferenceDeployment/Main.cpp b/FprimeZephyrReference/ReferenceDeployment/Main.cpp index c4d250c5..d6d7f462 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Main.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Main.cpp @@ -14,6 +14,8 @@ const struct device* ina219Sys = DEVICE_DT_GET(DT_NODELABEL(ina219_0)); const struct device* ina219Sol = DEVICE_DT_GET(DT_NODELABEL(ina219_1)); const struct device* serial = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart0)); const struct device* lora = DEVICE_DT_GET(DT_NODELABEL(lora0)); +const struct device* peripheral_uart = DEVICE_DT_GET(DT_NODELABEL(uart0)); +const struct device* peripheral_uart1 = DEVICE_DT_GET(DT_NODELABEL(uart1)); const struct device* lsm6dso = DEVICE_DT_GET(DT_NODELABEL(lsm6dso0)); const struct device* lis2mdl = DEVICE_DT_GET(DT_NODELABEL(lis2mdl0)); const struct device* rtc = DEVICE_DT_GET(DT_NODELABEL(rtc0)); @@ -36,6 +38,12 @@ int main(int argc, char* argv[]) { inputs.rtcDevice = rtc; inputs.baudRate = 115200; + // For the uart peripheral config + inputs.peripheralBaudRate = 115200; // Minimum is 19200 + inputs.peripheralUart = peripheral_uart; + inputs.peripheralBaudRate2 = 115200; // Minimum is 19200 + inputs.peripheralUart2 = peripheral_uart1; + // Setup, cycle, and teardown topology ReferenceDeployment::setupTopology(inputs); ReferenceDeployment::startRateGroups(); // Program loop diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 9b21a34f..ad805999 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -19,6 +19,10 @@ telemetry packets ReferenceDeploymentPackets { ComCcsds.commsBufferManager.EmptyBuffs ComCcsdsUart.commsBufferManager.NoBuffs ComCcsdsUart.commsBufferManager.EmptyBuffs + payloadBufferManager.NoBuffs + payloadBufferManager.EmptyBuffs + payloadBufferManager2.NoBuffs + payloadBufferManager2.EmptyBuffs ReferenceDeployment.rateGroup10Hz.RgCycleSlips ReferenceDeployment.rateGroup1Hz.RgCycleSlips } @@ -31,6 +35,12 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.TotalBuffs ComCcsdsUart.commsBufferManager.CurrBuffs ComCcsdsUart.comQueue.buffQueueDepth + payloadBufferManager.TotalBuffs + payloadBufferManager.CurrBuffs + payloadBufferManager.HiBuffs + payloadBufferManager2.TotalBuffs + payloadBufferManager2.CurrBuffs + payloadBufferManager2.HiBuffs CdhCore.tlmSend.SendLevel } @@ -85,6 +95,24 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.powerMonitor.TotalPowerGenerated } + packet CameraDebug id 11 group 5 { + ReferenceDeployment.cameraHandler.BytesReceived + ReferenceDeployment.cameraHandler.ExpectedSize + ReferenceDeployment.cameraHandler.IsReceiving + ReferenceDeployment.cameraHandler.FileOpen + ReferenceDeployment.cameraHandler.FileErrorCount + ReferenceDeployment.cameraHandler.ImagesSaved + } + + packet CameraDebug2 id 12 group 5 { + ReferenceDeployment.cameraHandler2.BytesReceived + ReferenceDeployment.cameraHandler2.ExpectedSize + ReferenceDeployment.cameraHandler2.IsReceiving + ReferenceDeployment.cameraHandler2.FileOpen + ReferenceDeployment.cameraHandler2.FileErrorCount + ReferenceDeployment.cameraHandler2.ImagesSaved + } + } omit { CdhCore.cmdDisp.CommandErrors # Only has one library, no custom versions diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index af8e02a4..0a96c23b 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -111,10 +111,15 @@ void setupTopology(const TopologyState& state) { lora.start(state.loraDevice, Zephyr::TransmitState::DISABLED); comDriver.configure(state.uartDevice, state.baudRate); + // UART from the board to the payload + peripheralUartDriver.configure(state.peripheralUart, state.peripheralBaudRate); lsm6dsoManager.configure(state.lsm6dsoDevice); lis2mdlManager.configure(state.lis2mdlDevice); ina219SysManager.configure(state.ina219SysDevice); ina219SolManager.configure(state.ina219SolDevice); + cameraHandler.configure(0); // Camera 0 + cameraHandler2.configure(1); // Camera 1 + peripheralUartDriver2.configure(state.peripheralUart2, state.peripheralBaudRate2); } void startRateGroups() { diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp index 00b11005..9bc496d8 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -69,17 +69,23 @@ namespace ReferenceDeployment { * autocoder. The contents are entirely up to the definition of the project. This deployment uses subtopologies. */ struct TopologyState { - const device* uartDevice; //!< UART device path for communication - const device* loraDevice; //!< LoRa device path for communication - U32 baudRate; //!< Baud rate for UART communication - CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore - ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds + + const device* uartDevice; //!< UART device path for communication + const device* loraDevice; //!< LoRa device path for communication + U32 baudRate; //!< Baud rate for UART communication + CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore + ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds + const device* peripheralUart; + U32 peripheralBaudRate; + const device* peripheralUart2; + U32 peripheralBaudRate2; FileHandling::SubtopologyState fileHandling; //!< Subtopology state for FileHandling const device* ina219SysDevice; //!< device path for battery board ina219 const device* ina219SolDevice; //!< device path for solar panel ina219 const device* lsm6dsoDevice; //!< LSM6DSO device path for accelerometer/gyroscope const device* lis2mdlDevice; //!< LIS2MDL device path for magnetometer const device* rtcDevice; //!< RTC device path + }; namespace PingEntries = ::PingEntries; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 099fdb45..b14fc6a9 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -37,16 +37,26 @@ module ReferenceDeployment { stack size Default.STACK_SIZE \ priority 3 - instance cmdSeq: Svc.CmdSequencer base id 0x10006000 \ + instance cmdSeq: Svc.CmdSequencer base id 0x10003000 \ queue size Default.QUEUE_SIZE \ stack size Default.STACK_SIZE \ priority 15 - instance prmDb: Svc.PrmDb base id 0x1000B000 \ + instance prmDb: Svc.PrmDb base id 0x10004000 \ queue size Default.QUEUE_SIZE \ stack size Default.STACK_SIZE \ priority 14 + instance payload: Components.PayloadCom base id 0x10005000 \ + queue size Default.QUEUE_SIZE \ + stack size Default.STACK_SIZE \ + priority 10 + + instance payload2: Components.PayloadCom base id 0x10006000 \ + queue size Default.QUEUE_SIZE \ + stack size Default.STACK_SIZE \ + priority 10 + # ---------------------------------------------------------------------- # Queued component instances # ---------------------------------------------------------------------- @@ -75,66 +85,117 @@ module ReferenceDeployment { instance lsm6dsoManager: Drv.Lsm6dsoManager base id 0x10019000 - instance bootloaderTrigger: Components.BootloaderTrigger base id 0x10020000 + instance bootloaderTrigger: Components.BootloaderTrigger base id 0x1001A000 - instance burnwire: Components.Burnwire base id 0x10021000 + instance burnwire: Components.Burnwire base id 0x1001B000 - instance gpioBurnwire0: Zephyr.ZephyrGpioDriver base id 0x10022000 + instance gpioBurnwire0: Zephyr.ZephyrGpioDriver base id 0x1001C000 - instance gpioBurnwire1: Zephyr.ZephyrGpioDriver base id 0x10023000 + instance gpioBurnwire1: Zephyr.ZephyrGpioDriver base id 0x1001D000 - instance comDelay: Components.ComDelay base id 0x10025000 + instance comDelay: Components.ComDelay base id 0x1001E000 - instance lora: Zephyr.LoRa base id 0x10026000 + instance lora: Zephyr.LoRa base id 0x1001F000 - instance comSplitterEvents: Svc.ComSplitter base id 0x10027000 + instance comSplitterEvents: Svc.ComSplitter base id 0x10020000 - instance comSplitterTelemetry: Svc.ComSplitter base id 0x10028000 + instance comSplitterTelemetry: Svc.ComSplitter base id 0x10021000 - instance antennaDeployer: Components.AntennaDeployer base id 0x10029000 + instance antennaDeployer: Components.AntennaDeployer base id 0x10022000 - instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x1002A000 + instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x10023000 - instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x1002B000 + instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x10024000 - instance gpioface1LS: Zephyr.ZephyrGpioDriver base id 0x1002C000 + instance gpioface1LS: Zephyr.ZephyrGpioDriver base id 0x10025000 - instance gpioface2LS: Zephyr.ZephyrGpioDriver base id 0x1002D000 + instance gpioface2LS: Zephyr.ZephyrGpioDriver base id 0x10026000 - instance gpioface3LS: Zephyr.ZephyrGpioDriver base id 0x1002E000 + instance gpioface3LS: Zephyr.ZephyrGpioDriver base id 0x10027000 - instance gpioface5LS: Zephyr.ZephyrGpioDriver base id 0x1002F000 + instance gpioface5LS: Zephyr.ZephyrGpioDriver base id 0x10028000 - instance gpioPayloadPowerLS: Zephyr.ZephyrGpioDriver base id 0x10030000 + instance gpioPayloadPowerLS: Zephyr.ZephyrGpioDriver base id 0x10029000 - instance gpioPayloadBatteryLS: Zephyr.ZephyrGpioDriver base id 0x10031000 + instance gpioPayloadBatteryLS: Zephyr.ZephyrGpioDriver base id 0x1002A000 - instance fsSpace: Components.FsSpace base id 0x10032000 + instance cameraHandler: Components.CameraHandler base id 0x1002B000 - instance face4LoadSwitch: Components.LoadSwitch base id 0x10033000 + instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x1002C000 - instance face0LoadSwitch: Components.LoadSwitch base id 0x10034000 + instance payloadBufferManager: Svc.BufferManager base id 0x1002D000 \ + { + phase Fpp.ToCpp.Phases.configObjects """ + Svc::BufferManager::BufferBins bins; + """ + phase Fpp.ToCpp.Phases.configComponents """ + memset(&ConfigObjects::ReferenceDeployment_payloadBufferManager::bins, 0, sizeof(ConfigObjects::ReferenceDeployment_payloadBufferManager::bins)); + // UART RX buffers for camera data streaming (4 KB, 2 buffers for ping-pong) + ConfigObjects::ReferenceDeployment_payloadBufferManager::bins.bins[0].bufferSize = 4 * 1024; + ConfigObjects::ReferenceDeployment_payloadBufferManager::bins.bins[0].numBuffers = 2; + ReferenceDeployment::payloadBufferManager.setup( + 1, // manager ID + 0, // store ID + ComCcsds::Allocation::memAllocator, // Reuse existing allocator from ComCcsds subtopology + ConfigObjects::ReferenceDeployment_payloadBufferManager::bins + ); + """ + phase Fpp.ToCpp.Phases.tearDownComponents """ + ReferenceDeployment::payloadBufferManager.cleanup(); + """ + } + instance fsSpace: Components.FsSpace base id 0x1002E000 - instance face1LoadSwitch: Components.LoadSwitch base id 0x10035000 + instance face4LoadSwitch: Components.LoadSwitch base id 0x1002F000 - instance face2LoadSwitch: Components.LoadSwitch base id 0x10036000 + instance face0LoadSwitch: Components.LoadSwitch base id 0x10030000 - instance face3LoadSwitch: Components.LoadSwitch base id 0x10037000 + instance face1LoadSwitch: Components.LoadSwitch base id 0x10031000 - instance face5LoadSwitch: Components.LoadSwitch base id 0x10038000 + instance face2LoadSwitch: Components.LoadSwitch base id 0x10032000 - instance payloadPowerLoadSwitch: Components.LoadSwitch base id 0x10039000 + instance face3LoadSwitch: Components.LoadSwitch base id 0x10033000 - instance payloadBatteryLoadSwitch: Components.LoadSwitch base id 0x1003A000 + instance face5LoadSwitch: Components.LoadSwitch base id 0x10034000 - instance resetManager: Components.ResetManager base id 0x1003B000 + instance payloadPowerLoadSwitch: Components.LoadSwitch base id 0x10035000 - instance powerMonitor: Components.PowerMonitor base id 0x1003C000 + instance payloadBatteryLoadSwitch: Components.LoadSwitch base id 0x10036000 - instance ina219SysManager: Drv.Ina219Manager base id 0x1003D000 + instance resetManager: Components.ResetManager base id 0x10037000 - instance ina219SolManager: Drv.Ina219Manager base id 0x1003E000 + instance powerMonitor: Components.PowerMonitor base id 0x10038000 - instance startupManager: Components.StartupManager base id 0x1003F000 + instance ina219SysManager: Drv.Ina219Manager base id 0x10039000 + + instance ina219SolManager: Drv.Ina219Manager base id 0x1003A000 + + instance startupManager: Components.StartupManager base id 0x1003B000 + + instance cameraHandler2: Components.CameraHandler base id 0x1003C000 + + instance peripheralUartDriver2: Zephyr.ZephyrUartDriver base id 0x1003D000 + + instance payloadBufferManager2: Svc.BufferManager base id 0x1003E000 \ + { + phase Fpp.ToCpp.Phases.configObjects """ + Svc::BufferManager::BufferBins bins; + """ + phase Fpp.ToCpp.Phases.configComponents """ + memset(&ConfigObjects::ReferenceDeployment_payloadBufferManager::bins, 0, sizeof(ConfigObjects::ReferenceDeployment_payloadBufferManager::bins)); + // UART RX buffers for camera data streaming (4 KB, 2 buffers for ping-pong) + ConfigObjects::ReferenceDeployment_payloadBufferManager::bins.bins[0].bufferSize = 4 * 1024; + ConfigObjects::ReferenceDeployment_payloadBufferManager::bins.bins[0].numBuffers = 2; + ReferenceDeployment::payloadBufferManager.setup( + 1, // manager ID + 0, // store ID + ComCcsds::Allocation::memAllocator, // Reuse existing allocator from ComCcsds subtopology + ConfigObjects::ReferenceDeployment_payloadBufferManager::bins + ); + """ + phase Fpp.ToCpp.Phases.tearDownComponents """ + ReferenceDeployment::payloadBufferManager.cleanup(); + """ + } } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 62099f02..f6e0fd54 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -61,6 +61,14 @@ module ReferenceDeployment { instance payloadPowerLoadSwitch instance payloadBatteryLoadSwitch instance fsSpace + instance payload + instance payload2 + instance cameraHandler + instance peripheralUartDriver + instance cameraHandler2 + instance peripheralUartDriver2 + instance payloadBufferManager + instance payloadBufferManager2 instance cmdSeq instance startupManager instance powerMonitor @@ -153,8 +161,11 @@ module ReferenceDeployment { rateGroup10Hz.RateGroupMemberOut[0] -> comDriver.schedIn rateGroup10Hz.RateGroupMemberOut[1] -> ComCcsdsUart.aggregator.timeout rateGroup10Hz.RateGroupMemberOut[2] -> ComCcsds.aggregator.timeout - rateGroup10Hz.RateGroupMemberOut[3] -> FileHandling.fileManager.schedIn - rateGroup10Hz.RateGroupMemberOut[4] -> cmdSeq.schedIn + rateGroup10Hz.RateGroupMemberOut[3] -> peripheralUartDriver.schedIn + + rateGroup10Hz.RateGroupMemberOut[4] -> FileHandling.fileManager.schedIn + rateGroup10Hz.RateGroupMemberOut[5] -> cmdSeq.schedIn + rateGroup10Hz.RateGroupMemberOut[6] -> peripheralUartDriver2.schedIn # Slow rate (1Hz) rate group rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1Hz] -> rateGroup1Hz.CycleIn @@ -168,9 +179,11 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[7] -> burnwire.schedIn rateGroup1Hz.RateGroupMemberOut[8] -> antennaDeployer.schedIn rateGroup1Hz.RateGroupMemberOut[9] -> fsSpace.run - rateGroup1Hz.RateGroupMemberOut[10] -> FileHandling.fileDownlink.Run - rateGroup1Hz.RateGroupMemberOut[11] -> startupManager.run - rateGroup1Hz.RateGroupMemberOut[12] -> powerMonitor.run + rateGroup1Hz.RateGroupMemberOut[10] -> payloadBufferManager.schedIn + rateGroup1Hz.RateGroupMemberOut[11] -> FileHandling.fileDownlink.Run + rateGroup1Hz.RateGroupMemberOut[12] -> startupManager.run + rateGroup1Hz.RateGroupMemberOut[13] -> powerMonitor.run + rateGroup1Hz.RateGroupMemberOut[14] -> payloadBufferManager2.schedIn } @@ -207,6 +220,40 @@ module ReferenceDeployment { imuManager.temperatureGet -> lsm6dsoManager.temperatureGet } + connections PayloadCom { + # PayloadCom <-> UART Driver + payload.uartForward -> peripheralUartDriver.$send + peripheralUartDriver.$recv -> payload.uartDataIn + + # Buffer return path (critical! - matches ComStub pattern) + payload.bufferReturn -> peripheralUartDriver.recvReturnIn + + # PayloadCom <-> CameraHandler data flow + payload.uartDataOut -> cameraHandler.dataIn + cameraHandler.commandOut -> payload.commandIn + + # UART driver allocates/deallocates from BufferManager + peripheralUartDriver.allocate -> payloadBufferManager.bufferGetCallee + peripheralUartDriver.deallocate -> payloadBufferManager.bufferSendIn + } + + connections PayloadCom2 { + # PayloadCom <-> UART Driver + payload2.uartForward -> peripheralUartDriver2.$send + peripheralUartDriver2.$recv -> payload2.uartDataIn + + # Buffer return path (critical! - matches ComStub pattern) + payload2.bufferReturn -> peripheralUartDriver2.recvReturnIn + + # PayloadCom <-> CameraHandler data flow + payload2.uartDataOut -> cameraHandler2.dataIn + cameraHandler2.commandOut -> payload2.commandIn + + # UART driver allocates/deallocates from BufferManager + peripheralUartDriver2.allocate -> payloadBufferManager.bufferGetCallee + peripheralUartDriver2.deallocate -> payloadBufferManager.bufferSendIn + } + connections ComCcsds_FileHandling { # File Downlink <-> ComQueue FileHandling.fileDownlink.bufferSendOut -> ComCcsdsUart.comQueue.bufferQueueIn[ComCcsds.Ports_ComBufferQueue.FILE] diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index 8eeefb4b..c88be742 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -13,7 +13,7 @@ constant PassiveRateGroupOutputPorts = 10 constant RateGroupDriverRateGroupPorts = 3 @ Used for command and registration ports -constant CmdDispatcherComponentCommandPorts = 30 +constant CmdDispatcherComponentCommandPorts = 35 @ Used for uplink/sequencer buffer/response ports constant CmdDispatcherSequencePorts = 5 diff --git a/FprimeZephyrReference/project/config/FpConstants.fpp b/FprimeZephyrReference/project/config/FpConstants.fpp index e91467a3..5b112b61 100644 --- a/FprimeZephyrReference/project/config/FpConstants.fpp +++ b/FprimeZephyrReference/project/config/FpConstants.fpp @@ -51,7 +51,7 @@ constant FW_PARAM_BUFFER_MAX_SIZE = FW_COM_BUFFER_MAX_SIZE - SIZE_OF_FwPrmIdType constant FW_PARAM_STRING_MAX_SIZE = 40 @ Specifies the maximum size of a file downlink chunk -constant FW_FILE_BUFFER_MAX_SIZE = FW_COM_BUFFER_MAX_SIZE +constant FW_FILE_BUFFER_MAX_SIZE = FW_COM_BUFFER_MAX_SIZE - 6 @ Specifies the maximum size of a string in an interface call constant FW_INTERNAL_INTERFACE_STRING_MAX_SIZE = 256 diff --git a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 68e52a32..9ce37a32 100644 --- a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp +++ b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp @@ -16,7 +16,7 @@ #include namespace Svc { -static const FwChanIdType MAX_PACKETIZER_PACKETS = 10; +static const FwChanIdType MAX_PACKETIZER_PACKETS = 15; static const FwChanIdType TLMPACKETIZER_NUM_TLM_HASH_SLOTS = 15; // !< Number of slots in the hash table. // Works best when set to about twice the number of components producing telemetry @@ -24,7 +24,7 @@ static const FwChanIdType TLMPACKETIZER_HASH_MOD_VALUE = 999; // !< The modulo value of the hashing function. // Should be set to a little below the ID gaps to spread the entries around -static const FwChanIdType TLMPACKETIZER_HASH_BUCKETS = 90; // !< Buckets assignable to a hash slot. +static const FwChanIdType TLMPACKETIZER_HASH_BUCKETS = 110; // !< Buckets assignable to a hash slot. // Buckets must be >= number of telemetry channels in system static const FwChanIdType TLMPACKETIZER_MAX_MISSING_TLM_CHECK = 25; // !< Maximum number of missing telemetry channel checks diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi index 48167dd4..ce52e817 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5-pinctrl.dtsi @@ -11,6 +11,26 @@ input-enable; }; }; + uart0_default: uart0_default { + group1 { + pinmux = ; + }; + + group2 { + pinmux = ; + input-enable; + }; + }; + uart1_default: uart1_default { + group1 { + pinmux = ; + }; + + group2 { + pinmux = ; + input-enable; + }; + }; spi1_default: spi1_default { group1 { pinmux = , ; diff --git a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi index 1141469b..9562dab9 100644 --- a/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi +++ b/boards/bronco_space/proves_flight_control_board_v5/proves_flight_control_board_v5.dtsi @@ -100,6 +100,20 @@ zephyr_udc0: &usbd { status = "okay"; }; +&uart0 { + status = "okay"; + pinctrl-0 = <&uart0_default>; + current-speed = <115200>; + pinctrl-names = "default"; +}; + +&uart1 { + status = "okay"; + pinctrl-0 = <&uart1_default>; + current-speed = <115200>; + pinctrl-names = "default"; +}; + &spi0 { status = "okay"; cs-gpios = <&gpio0 15 GPIO_ACTIVE_LOW>; diff --git a/micro-python/camera-main.py b/micro-python/camera-main.py new file mode 100644 index 00000000..b1c95ad5 --- /dev/null +++ b/micro-python/camera-main.py @@ -0,0 +1,420 @@ +"""OpenMV Nicla Vision - UART Image Transfer. + +Compatible with PayloadHandler component +Protocol: [4-byte uint32][JPEG data] + +Notes: +- This version is defensive about UART writes/reads and checks ACK contents. +- It attempts to be robust to boot timing and power-related issues. + +NOTE: If code crashes, remove optional type hints. +""" + +import struct +import time + +import sensor +from pyb import LED, UART + + +# --- Utility functions --- +def blink_led(led: LED, duration_ms: int = 100) -> None: + """Blink an LED once. + + Args: + led: LED object to blink + duration_ms: How long to keep LED on (in milliseconds) + """ + led.on() + time.sleep_ms(duration_ms) # type: ignore[attr-defined] + led.off() + + +def write_all(uart_obj: UART, data: bytes, write_timeout_ms: int = 2000) -> bool: + """Ensure all bytes in `data` are written to UART. + + Returns True on success, False on timeout/partial write. + """ + total = len(data) + off = 0 + start = time.ticks_ms() # type: ignore[attr-defined] + while off < total: + written = uart_obj.write(data[off:]) # may return number of bytes written or None + if written is None: + written = 0 + off += written + # If nothing written, check timeout + if written == 0 and time.ticks_diff(time.ticks_ms(), start) > write_timeout_ms: # type: ignore[attr-defined] + return False + return True + + +# --- UART Setup --- +# Use a per-character timeout (ms) when reading; we'll also use a total-timeout mechanism. +uart = UART("LP1", 115200, timeout=1000) # 1s inter-char timeout (OpenMV UART) + +# --- LEDs --- +red = LED(1) # error indicator +green = LED(2) # activity (command received) +blue = LED(3) # idle heartbeat + +# --- STATE MACHINE --- +STATE_IDLE = 0 +STATE_CAPTURE = 1 +STATE_SEND = 2 +STATE_ERROR = 3 + +# Set global state +state = STATE_IDLE + +# Track current framesize +current_framesize = sensor.QVGA # Default to QVGA +QUALITY = 90 + +# --- Camera Setup --- +# Add delay for hardware to stabilize when running standalone +time.sleep_ms(500) # type: ignore[attr-defined] +image_count = 0 + +# Initialize camera with error handling for standalone operation +try: + sensor.reset() + # Brief pause after reset + time.sleep_ms(100) # type: ignore[attr-defined] + sensor.set_pixformat(sensor.RGB565) + sensor.set_framesize(sensor.QVGA) + + try: + sensor.skip_frames(n=30) # Skip 30 frames to let auto-exposure settle + except RuntimeError as e: + # If skip_frames times out (common when running standalone), + # just continue - the camera will initialize on first actual snapshot() + print("WARNING: skip_frames timed out (normal for standalone), camera will init on first capture: {}".format(e)) + time.sleep_ms(200) # type: ignore[attr-defined] +except Exception as e: + print("ERROR: Camera initialization failed:", e) + # Flash red LED to indicate camera init failure + for _ in range(5): + blink_led(red, 100) + time.sleep_ms(100) # type: ignore[attr-defined] + raise # Re-raise to stop execution if camera can't initialize + + +# --- Communication functions --- + + +def is_ack(ack_bytes: bytes) -> bool: + r"""Decide whether the received bytes qualify as an ACK. + + Accepts common variants like: + b"\n", b"\r\n", b"MOISES\n", b"ACK\n" + """ + if not ack_bytes: + return False + # work with uppercase, strip whitespace + try: + s = ack_bytes.strip().upper() + except Exception: + try: + s = bytes(ack_bytes).strip().upper() + except Exception: + return False + + # common acceptable ACK tokens + if b"" in s or s == b"" or s.startswith(b""): + return True + if s == b"MOISES" or s.startswith(b"MOISES"): + return True + if s == b"ACK" or s.startswith(b"ACK"): + return True + return False + + +def wait_for_ack(total_timeout_ms: int = 3000) -> bytes | None: + """Wait for an ACK line from UART up to total_timeout_ms. + + Returns the received bytes (non-empty) on success, or None on timeout. + """ + deadline = time.ticks_add(time.ticks_ms(), total_timeout_ms) # type: ignore[attr-defined] + # We will use uart.readline() which returns bytes up to newline (or None on timeout) + while time.ticks_diff(deadline, time.ticks_ms()) > 0: # type: ignore[attr-defined] + try: + line = uart.readline() + except Exception: + line = None + + if line: + # debug print (to USB console if connected) + try: + print("RX:", repr(line)) + except Exception: + pass + + if is_ack(line): + return line + # not a recognized ack -> continue reading until timeout (ignore other chatter) + # small sleep to yield CPU + time.sleep_ms(10) # type: ignore[attr-defined] + # timed out + return None + + +def send_image_protocol(jpeg_bytes: bytes) -> bool: + """Send JPEG image bytes using protocol with ACK handshake. + + Protocol: [4-byte LE uint32][data chunks with ACK] + """ + # Get image size + file_size = len(jpeg_bytes) + + if file_size == 0: + print("ERROR: empty JPEG data") + return False + + print("=== Starting image transfer ===") + print("JPEG size: {} bytes".format(file_size)) + + # Build header in one buffer + try: + header = b"" + struct.pack("" + except Exception as e: + print("ERROR: building header:", e) + return False + + # Send header + if not write_all(uart, header): + print("ERROR: header write failed/timeout") + blink_led(red, 200) + return False + + # Wait for ACK after header + ack = wait_for_ack(total_timeout_ms=3000) + if not ack: + print("ERROR: No ACK after header") + blink_led(red, 300) + return False + + # Send image data in chunks with ACK handshake + chunk_size = 64 # relatively small - adjust if necessary + bytes_sent = 0 + offset = 0 + + try: + while offset < file_size: + # Python slicing handles case where end_index exceeds the list length. It just returns the remaining items. + chunk = jpeg_bytes[offset : offset + chunk_size] + if not chunk: + break + # write and ensure all bytes go out + if not write_all(uart, chunk): + print("ERROR: chunk write timeout") + blink_led(red, 300) + return False + bytes_sent += len(chunk) + offset += len(chunk) + # Wait for ACK after each chunk (or at least make sure receiver is ready) + ack = wait_for_ack(total_timeout_ms=2000) + if not ack: + print("ERROR: No ACK after chunk (bytes_sent={})".format(bytes_sent)) + blink_led(red, 300) + return False + except Exception as e: + print("ERROR: sending JPEG data:", e) + blink_led(red, 300) + return False + + # Send footer + if not write_all(uart, b""): + print("ERROR: footer write failed") + blink_led(red, 300) + return False + + # Final ACK + if wait_for_ack(total_timeout_ms=3000): + # flash green LED to show success + for _ in range(3): + blink_led(green, 100) + time.sleep_ms(100) # type: ignore[attr-defined] + return True + else: + print("ERROR: No final ACK") + blink_led(red, 300) + return False + + +def ping_handler() -> bool: + """Respond to ping command with pong message.""" + try: + # Send pong response over UART + response = b"PONG\n" + if write_all(uart, response): + print("Ping received, sent PONG") + # Quick green LED flash to indicate ping response + blink_led(green, duration_ms=50) + return True + else: + print("ERROR: Failed to send PONG response") + return False + except Exception as e: + print("ERROR in ping_handler():", e) + return False + + +def snap_handler() -> None: + blue.off() + """Capture JPEG image in memory and send over UART""" + global state + if state != STATE_IDLE: + print("WARNING: snap command received while not idle. Ignoring.") + return + + state = STATE_CAPTURE + + try: + # small pause to ensure camera ready + time.sleep_ms(50) # type: ignore[attr-defined] + img = sensor.snapshot() + + jpeg_params = {"quality": QUALITY, "encode_for_ide": False} + + print("Snapping with quality: {}".format(QUALITY)) + + # Try to convert to JPEG, with fallback to lower quality if needed + jpeg_bytes = None + try: + jpeg_bytes = img.to_jpeg(**jpeg_params).bytearray() + except Exception as e: + error_msg = str(e) + # If frame buffer error and we're using high quality, try lower quality + if "frame buffer" in error_msg.lower() and QUALITY > 50: + print("WARNING: High quality failed, trying lower quality (50)...") + try: + jpeg_params["quality"] = 50 + jpeg_bytes = img.to_jpeg(**jpeg_params).bytearray() + except Exception as e2: + print("ERROR: Failed to get JPEG data even at lower quality:", e2) + blink_led(red, 200) + state = STATE_ERROR + return + else: + print("ERROR: Failed to get JPEG data from image:", e) + blink_led(red, 200) + state = STATE_ERROR + return + + if not jpeg_bytes or len(jpeg_bytes) == 0: + print("ERROR: Failed to get JPEG data from image") + blink_led(red, 200) + state = STATE_ERROR + return + + print("Captured JPEG: {} bytes".format(len(jpeg_bytes))) + + state = STATE_SEND + success = send_image_protocol(jpeg_bytes) + + # Log result + if success: + print("SUCCESS: JPEG sent ({} bytes)".format(len(jpeg_bytes))) + state = STATE_IDLE + else: + print("FAILED: JPEG transfer failed ({} bytes)".format(len(jpeg_bytes))) + state = STATE_ERROR + + except Exception as e: + print("ERROR in snap():", e) + blink_led(red, 200) + state = STATE_ERROR + + +# --- Main Loop --- + +# Brief startup logging (no noisy startup protocol messages) +print("\n=== Camera Started ===") + +# Flush any leftover UART data from previous runs (defensive) +try: + while uart.any(): + uart.read() +except Exception: + pass + +time.sleep_ms(100) # type: ignore[attr-defined] +print("Ready for commands...") + +heartbeat_ms = 1000 +last_hb = time.ticks_ms() # type: ignore[attr-defined] + +COMMANDS = { + "snap": snap_handler, + "ping": ping_handler, +} + +DEBUG = False + +if DEBUG: + from pyb import USB_VCP + + try: + usb = USB_VCP() + usb.setinterrupt(-1) + except Exception as e: + print(f"Error using usb: {e}") + + +while True: + # Read line (non-blocking-ish due to UART timeout param) + if state == STATE_IDLE: + try: + msg = uart.readline() + if DEBUG: + msg = usb.readline() + except Exception: + msg = None + + if msg: + # try to decode, but allow bytes processing if decode fails + try: + text = msg.decode("utf-8", "ignore").strip() + text = text.replace("\x00", "").strip() + except Exception: + text = "" + + if text: + # blink green LED for received command + blink_led(green, 80) + print("Command received: '{}'".format(text)) + + # Commands + cmd = text.lower() + handler = COMMANDS.get(cmd) + if handler is None: + print("Unknown command: '{}'".format(text)) + # Unknown commands + pass + else: + try: + handler() + except Exception as e: + print("ERROR in handler:", e) + blink_led(red, 200) + state = STATE_ERROR + pass + + elif state == STATE_ERROR: + # TODO: How should we handle errors? + print("Recovering from error...") + time.sleep_ms(500) # type: ignore[attr-defined] + state = STATE_IDLE + + # Idle heartbeat + if time.ticks_diff(time.ticks_ms(), last_hb) >= heartbeat_ms: # type: ignore[attr-defined] + try: + blue.toggle() + except Exception: + # some platforms may not have toggle; emulate + blink_led(blue, 20) + last_hb = time.ticks_ms() # type: ignore[attr-defined] + + # yield tiny amount of CPU + time.sleep_ms(10) # type: ignore[attr-defined] diff --git a/micro-python/pyproject.toml b/micro-python/pyproject.toml new file mode 100644 index 00000000..f3493a01 --- /dev/null +++ b/micro-python/pyproject.toml @@ -0,0 +1,65 @@ +[tool.black] + +line-length = 120 +target-version = ["py310"] +include = '\.pyi?$' + +[tool.mypy] + +pretty = true +show_column_numbers = true +show_error_context = true +show_error_codes = true +show_traceback = true +disallow_untyped_defs = true +strict_equality = true +allow_redefinition = true + +warn_unused_ignores = true +warn_redundant_casts = true + +incremental = true +namespace_packages = false + +[[tool.mypy.overrides]] + +module = [ + "sensor", + "pyb", +] +ignore_missing_imports = true + +[tool.isort] + +profile = "black" + +[tool.ruff] + +line-length = 120 +target-version = "py310" + +[tool.ruff.lint] + +select = ["ANN", "D", "E", "F", "G", "I", "N", "PGH", "PLC", "PLE", "PLR", "PLW", "W"] + +ignore = [ + "D101", "D102", "D103", "D104", "D105", "D106", "D107", + "N812", "N817", + "PLR0911", "PLR0912", "PLR0913", "PLR0915", "PLR2004", + "PLW0603", "PLW2901", "PLC0415", +] + +dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" + +[tool.ruff.lint.per-file-ignores] + +"__init__.py" = ["E402", "F401", "F403", "F811"] + +[tool.ruff.lint.isort] + +known-first-party = ["assets", "tests"] +combine-as-imports = true + +[tool.ruff.lint.pydocstyle] + +convention = "google"