From 4949f90b79d00ae28aa640aa800f177e23c3df0f Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Sat, 1 Nov 2025 16:24:36 -0400 Subject: [PATCH 01/34] Initial Component Design --- .../Components/CMakeLists.txt | 1 + .../Components/CameraManager/CMakeLists.txt | 36 ++++++++++ .../CameraManager/CameraManager.cpp | 39 +++++++++++ .../CameraManager/CameraManager.fpp | 66 +++++++++++++++++++ .../CameraManager/CameraManager.hpp | 44 +++++++++++++ .../Components/CameraManager/docs/sdd.md | 66 +++++++++++++++++++ 6 files changed, 252 insertions(+) create mode 100644 FprimeZephyrReference/Components/CameraManager/CMakeLists.txt create mode 100644 FprimeZephyrReference/Components/CameraManager/CameraManager.cpp create mode 100644 FprimeZephyrReference/Components/CameraManager/CameraManager.fpp create mode 100644 FprimeZephyrReference/Components/CameraManager/CameraManager.hpp create mode 100644 FprimeZephyrReference/Components/CameraManager/docs/sdd.md diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index 26405d0b..889f4c5a 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -10,3 +10,4 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Burnwire/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/BootloaderTrigger/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/AntennaDeployer/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/FsSpace/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/CameraManager/") diff --git a/FprimeZephyrReference/Components/CameraManager/CMakeLists.txt b/FprimeZephyrReference/Components/CameraManager/CMakeLists.txt new file mode 100644 index 00000000..460ecc8a --- /dev/null +++ b/FprimeZephyrReference/Components/CameraManager/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}/CameraManager.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/CameraManager.cpp" +# DEPENDS +# MyPackage_MyOtherModule +) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/CameraManager.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/CameraManagerTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/CameraManagerTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp new file mode 100644 index 00000000..23b091a1 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp @@ -0,0 +1,39 @@ +// ====================================================================== +// \title CameraManager.cpp +// \author robertpendergrast +// \brief cpp file for CameraManager component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/CameraManager/CameraManager.hpp" + +namespace FprimeZephyrReference { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +CameraManager ::CameraManager(const char* const compName) : CameraManagerComponentBase(compName) {} + +CameraManager ::~CameraManager() {} + +// ---------------------------------------------------------------------- +// Handler implementations for commands +// ---------------------------------------------------------------------- + +void CameraManager ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + // Prepare the "snap" command to send over UART via out_port + Fw::Buffer snapBuffer(0, reinterpret_cast(snap_cmd), sizeof(snap_cmd) - 1); // exclude null terminator + // Send the buffer via out_port, check/send status if needed + Drv::ByteStreamStatus sendStatus = this->out_port_out(0, snapBuffer); + if (sendStatus != Drv::ByteStreamStatus::OP_OK) { + this->log_WARNING_HI_TakeImageError(); + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); + return; + } + else { + this->log_ACTIVITY_HI_PictureTaken(); + } + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + +} // namespace FprimeZephyrReference diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp new file mode 100644 index 00000000..9573322c --- /dev/null +++ b/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp @@ -0,0 +1,66 @@ +module FprimeZephyrReference { + @ Manager for Nicla Vision + active component CameraManager { + + # One async command/port is required for active components + # This should be overridden by the developers with a useful command/port + @ TODO + async command TAKE_IMAGE + + event TakeImageError() severity warning high format "Failed to take picture" + + event PictureTaken() severity activity high format "Picture Taken" + + output port out_port: Drv.ByteStreamSend + + ############################################################################## + #### 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 + + } +} \ No newline at end of file diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp new file mode 100644 index 00000000..c3fc7e07 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp @@ -0,0 +1,44 @@ +// ====================================================================== +// \title CameraManager.hpp +// \author robertpendergrast +// \brief hpp file for CameraManager component implementation class +// ====================================================================== + +#ifndef FprimeZephyrReference_CameraManager_HPP +#define FprimeZephyrReference_CameraManager_HPP + +#include "FprimeZephyrReference/Components/CameraManager/CameraManagerComponentAc.hpp" + +namespace FprimeZephyrReference { + +class CameraManager final : public CameraManagerComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct CameraManager object + CameraManager(const char* const compName //!< The component name + ); + + //! Destroy CameraManager object + ~CameraManager(); + + const char snap_cmd[4] = {'s', 'n', 'a', 'p'}; + + private: + // ---------------------------------------------------------------------- + // Handler implementations for commands + // ---------------------------------------------------------------------- + + //! Handler implementation for command TAKE_IMAGE + //! + //! TODO + void TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; +}; + +} // namespace FprimeZephyrReference + +#endif diff --git a/FprimeZephyrReference/Components/CameraManager/docs/sdd.md b/FprimeZephyrReference/Components/CameraManager/docs/sdd.md new file mode 100644 index 00000000..6cca5111 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraManager/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 From 10089dd24c273dc9f14728e4ea126d77959d8c8c Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Sun, 2 Nov 2025 14:58:02 -0500 Subject: [PATCH 02/34] Camera works kinda --- .../Components/CameraManager/CameraManager.cpp | 11 +++++++++-- .../Components/CameraManager/CameraManager.fpp | 6 +++--- .../Components/CameraManager/CameraManager.hpp | 4 ++-- FprimeZephyrReference/ReferenceDeployment/Main.cpp | 5 +++++ .../Top/ReferenceDeploymentTopology.cpp | 3 +++ .../Top/ReferenceDeploymentTopologyDefs.hpp | 2 ++ .../ReferenceDeployment/Top/instances.fpp | 6 ++++++ .../ReferenceDeployment/Top/topology.fpp | 6 ++++++ .../proves_flight_control_board_v5-pinctrl.dtsi | 10 ++++++++++ .../proves_flight_control_board_v5.dtsi | 7 +++++++ 10 files changed, 53 insertions(+), 7 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp index 23b091a1..317e69e8 100644 --- a/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp +++ b/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp @@ -6,7 +6,7 @@ #include "FprimeZephyrReference/Components/CameraManager/CameraManager.hpp" -namespace FprimeZephyrReference { +namespace Components { // ---------------------------------------------------------------------- // Component construction and destruction @@ -21,10 +21,16 @@ CameraManager ::~CameraManager() {} // ---------------------------------------------------------------------- void CameraManager ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + // Prepare the "snap" command to send over UART via out_port - Fw::Buffer snapBuffer(0, reinterpret_cast(snap_cmd), sizeof(snap_cmd) - 1); // exclude null terminator + const U8 size = sizeof(this->snapArray); + + // Create a buffer that references this array + Fw::Buffer snapBuffer(this->snapArray, size); + // Send the buffer via out_port, check/send status if needed Drv::ByteStreamStatus sendStatus = this->out_port_out(0, snapBuffer); + if (sendStatus != Drv::ByteStreamStatus::OP_OK) { this->log_WARNING_HI_TakeImageError(); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); @@ -33,6 +39,7 @@ void CameraManager ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { else { this->log_ACTIVITY_HI_PictureTaken(); } + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp index 9573322c..955a21c4 100644 --- a/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp +++ b/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp @@ -1,11 +1,11 @@ -module FprimeZephyrReference { +module Components { @ Manager for Nicla Vision - active component CameraManager { + passive component CameraManager { # One async command/port is required for active components # This should be overridden by the developers with a useful command/port @ TODO - async command TAKE_IMAGE + sync command TAKE_IMAGE event TakeImageError() severity warning high format "Failed to take picture" diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp index c3fc7e07..6a36c37e 100644 --- a/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp +++ b/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp @@ -9,7 +9,7 @@ #include "FprimeZephyrReference/Components/CameraManager/CameraManagerComponentAc.hpp" -namespace FprimeZephyrReference { +namespace Components { class CameraManager final : public CameraManagerComponentBase { public: @@ -24,7 +24,7 @@ class CameraManager final : public CameraManagerComponentBase { //! Destroy CameraManager object ~CameraManager(); - const char snap_cmd[4] = {'s', 'n', 'a', 'p'}; + U8 snapArray[5] = {'s', 'n', 'a', 'p', '\n'}; private: // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/ReferenceDeployment/Main.cpp b/FprimeZephyrReference/ReferenceDeployment/Main.cpp index 705a7f2e..89de7cfc 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Main.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Main.cpp @@ -12,6 +12,7 @@ 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)); int main(int argc, char* argv[]) { // ** DO NOT REMOVE **// @@ -26,6 +27,10 @@ int main(int argc, char* argv[]) { inputs.uartDevice = serial; inputs.baudRate = 115200; + // For the uart peripheral config + inputs.peripheralBaudRate = 115200; // Minimum is 19200 + inputs.peripheralUart = peripheral_uart; + // Setup, cycle, and teardown topology ReferenceDeployment::setupTopology(inputs); ReferenceDeployment::startRateGroups(); // Program loop diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index ccef9075..d2d70cec 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -87,6 +87,9 @@ void setupTopology(const TopologyState& state) { // for over-the-air communications. 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); } void startRateGroups() { diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp index 81064f65..1df28b32 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -74,6 +74,8 @@ struct TopologyState { 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; }; namespace PingEntries = ::PingEntries; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 26eebbf7..f00ad64c 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -89,4 +89,10 @@ module ReferenceDeployment { instance antennaDeployer: Components.AntennaDeployer base id 0x10029000 instance fsSpace: Components.FsSpace base id 0x10030000 + + instance camera: Components.CameraManager base id 0x10031000 + + instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x10032000 + + } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index ec3bcfb5..4b6d2131 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -44,6 +44,8 @@ module ReferenceDeployment { # For UART sideband communication instance comDriver instance fsSpace + instance camera + instance peripheralUartDriver # ---------------------------------------------------------------------- @@ -161,5 +163,9 @@ module ReferenceDeployment { imuManager.magneticFieldGet -> lis2mdlManager.magneticFieldGet imuManager.temperatureGet -> lsm6dsoManager.temperatureGet } + + connections Camera { + camera.out_port -> peripheralUartDriver.$send + } } } 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 34fd8f3c..65637823 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,16 @@ input-enable; }; }; + uart0_default: uart0_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 11db77df..2af1cbbf 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,13 @@ zephyr_udc0: &usbd { status = "okay"; }; +&uart0 { + status = "okay"; + pinctrl-0 = <&uart0_default>; + current-speed = <115200>; + pinctrl-names = "default"; +}; + &spi0 { status = "okay"; cs-gpios = <&gpio0 15 GPIO_ACTIVE_LOW>; From 307f56eccb638906b17c2b3a3f0f6d49a3997ee1 Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Mon, 3 Nov 2025 15:20:38 -0500 Subject: [PATCH 03/34] Basic Reading Implementation (untested) --- .../Components/CMakeLists.txt | 2 +- .../CameraManager/CameraManager.cpp | 46 ------ .../CameraManager/CameraManager.hpp | 44 ------ .../CMakeLists.txt | 10 +- .../PayloadHandler/PayloadHandler.cpp | 140 ++++++++++++++++++ .../PayloadHandler.fpp} | 12 +- .../PayloadHandler/PayloadHandler.hpp | 66 +++++++++ .../docs/sdd.md | 0 .../ReferenceDeployment/Top/instances.fpp | 2 +- .../ReferenceDeployment/Top/topology.fpp | 7 +- ...roves_flight_control_board_v5-pinctrl.dtsi | 10 ++ .../proves_flight_control_board_v5.dtsi | 7 + 12 files changed, 242 insertions(+), 104 deletions(-) delete mode 100644 FprimeZephyrReference/Components/CameraManager/CameraManager.cpp delete mode 100644 FprimeZephyrReference/Components/CameraManager/CameraManager.hpp rename FprimeZephyrReference/Components/{CameraManager => PayloadHandler}/CMakeLists.txt (72%) create mode 100644 FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp rename FprimeZephyrReference/Components/{CameraManager/CameraManager.fpp => PayloadHandler/PayloadHandler.fpp} (79%) create mode 100644 FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp rename FprimeZephyrReference/Components/{CameraManager => PayloadHandler}/docs/sdd.md (100%) diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index 889f4c5a..e9d6171d 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -10,4 +10,4 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Burnwire/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/BootloaderTrigger/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/AntennaDeployer/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/FsSpace/") -add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/CameraManager/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/PayloadHandler/") diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp deleted file mode 100644 index 317e69e8..00000000 --- a/FprimeZephyrReference/Components/CameraManager/CameraManager.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// ====================================================================== -// \title CameraManager.cpp -// \author robertpendergrast -// \brief cpp file for CameraManager component implementation class -// ====================================================================== - -#include "FprimeZephyrReference/Components/CameraManager/CameraManager.hpp" - -namespace Components { - -// ---------------------------------------------------------------------- -// Component construction and destruction -// ---------------------------------------------------------------------- - -CameraManager ::CameraManager(const char* const compName) : CameraManagerComponentBase(compName) {} - -CameraManager ::~CameraManager() {} - -// ---------------------------------------------------------------------- -// Handler implementations for commands -// ---------------------------------------------------------------------- - -void CameraManager ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - - // Prepare the "snap" command to send over UART via out_port - const U8 size = sizeof(this->snapArray); - - // Create a buffer that references this array - Fw::Buffer snapBuffer(this->snapArray, size); - - // Send the buffer via out_port, check/send status if needed - Drv::ByteStreamStatus sendStatus = this->out_port_out(0, snapBuffer); - - if (sendStatus != Drv::ByteStreamStatus::OP_OK) { - this->log_WARNING_HI_TakeImageError(); - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); - return; - } - else { - this->log_ACTIVITY_HI_PictureTaken(); - } - - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); -} - -} // namespace FprimeZephyrReference diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp b/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp deleted file mode 100644 index 6a36c37e..00000000 --- a/FprimeZephyrReference/Components/CameraManager/CameraManager.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// ====================================================================== -// \title CameraManager.hpp -// \author robertpendergrast -// \brief hpp file for CameraManager component implementation class -// ====================================================================== - -#ifndef FprimeZephyrReference_CameraManager_HPP -#define FprimeZephyrReference_CameraManager_HPP - -#include "FprimeZephyrReference/Components/CameraManager/CameraManagerComponentAc.hpp" - -namespace Components { - -class CameraManager final : public CameraManagerComponentBase { - public: - // ---------------------------------------------------------------------- - // Component construction and destruction - // ---------------------------------------------------------------------- - - //! Construct CameraManager object - CameraManager(const char* const compName //!< The component name - ); - - //! Destroy CameraManager object - ~CameraManager(); - - U8 snapArray[5] = {'s', 'n', 'a', 'p', '\n'}; - - private: - // ---------------------------------------------------------------------- - // Handler implementations for commands - // ---------------------------------------------------------------------- - - //! Handler implementation for command TAKE_IMAGE - //! - //! TODO - void TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq //!< The command sequence number - ) override; -}; - -} // namespace FprimeZephyrReference - -#endif diff --git a/FprimeZephyrReference/Components/CameraManager/CMakeLists.txt b/FprimeZephyrReference/Components/PayloadHandler/CMakeLists.txt similarity index 72% rename from FprimeZephyrReference/Components/CameraManager/CMakeLists.txt rename to FprimeZephyrReference/Components/PayloadHandler/CMakeLists.txt index 460ecc8a..15cabfdd 100644 --- a/FprimeZephyrReference/Components/CameraManager/CMakeLists.txt +++ b/FprimeZephyrReference/Components/PayloadHandler/CMakeLists.txt @@ -16,9 +16,9 @@ register_fprime_library( AUTOCODER_INPUTS - "${CMAKE_CURRENT_LIST_DIR}/CameraManager.fpp" + "${CMAKE_CURRENT_LIST_DIR}/PayloadHandler.fpp" SOURCES - "${CMAKE_CURRENT_LIST_DIR}/CameraManager.cpp" + "${CMAKE_CURRENT_LIST_DIR}/PayloadHandler.cpp" # DEPENDS # MyPackage_MyOtherModule ) @@ -26,10 +26,10 @@ register_fprime_library( ### Unit Tests ### # register_fprime_ut( # AUTOCODER_INPUTS -# "${CMAKE_CURRENT_LIST_DIR}/CameraManager.fpp" +# "${CMAKE_CURRENT_LIST_DIR}/PayloadHandler.fpp" # SOURCES -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/CameraManagerTestMain.cpp" -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/CameraManagerTester.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/PayloadHandlerTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/PayloadHandlerTester.cpp" # DEPENDS # STest # For rules-based testing # UT_AUTO_HELPERS diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp new file mode 100644 index 00000000..1930bd29 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -0,0 +1,140 @@ +// ====================================================================== +// \title PayloadHandler.cpp +// \author robertpendergrast +// \brief cpp file for PayloadHandler component implementation class +// ====================================================================== +#include "Os/File.hpp" +#include "FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp" + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +PayloadHandler ::PayloadHandler(const char* const compName) : PayloadHandlerComponentBase(compName) {} +PayloadHandler ::~PayloadHandler() {} + + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + + +void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { + + const U8* data = buffer.getData(); + FwSizeType size = buffer.getSize(); + + for (FwSizeType i = 0; i < size; i++) { + // Process each byte of data as needed + U8 byte = data[i]; + + + if (!m_receiving){ + // We are not currently receiving a file + + // Append byte to line buffer: This is how we check the header to determine data type + if (m_lineIndex < sizeof(m_lineBuffer) - 1) { + m_lineBuffer[m_lineIndex++] = byte; + } + + // Have we reached the end of the line? If so that means we have a header + // Check to see what the header is. + if (byte == '\n' || byte == '\r') { + m_lineBuffer[m_lineIndex] = 0; // Null-terminate + m_lineIndex = 0; + + // Check the header. + // Right now I'm just checking for an image start tag, but we can expand this to other types later + if (strstr((const char*)m_lineBuffer, "")) { + m_receiving = true; + m_bytes_received = 0; + m_expected_size = 0; + continue; + } + + // If in receiving mode and expected size not set, this line is the size + if (m_receiving && m_expected_size == 0) { + + // First we set the expected size + m_expected_size = atoi((const char*)m_lineBuffer); + + + // Then we open the file to write to, which we will be writing to over a lot of iterations + if (m_data_file_count >= 9) { + m_data_file_count = 0; + } + + char filenameBuffer[20]; + snprintf(filenameBuffer, sizeof(filenameBuffer), "payload_%d.jpg", m_data_file_count); + m_currentFilename = filenameBuffer; + + // Open the file and prepare to write in the next iteration + Os::File::Status fileStatus = m_file.open(m_currentFilename.c_str(), Os::File::OPEN_CREATE, Os::File::OVERWRITE); + if (fileStatus != Os::File::OP_OK) { + m_receiving = false; + continue; + } + continue; + } + } + + + } else if (m_bytes_received < m_expected_size){ + // We are currently receiving a file + + // Cast byte to a buffer + // Write a byte to the file + FwSizeType oneByte = 1; + m_file.write(&byte, oneByte); + m_bytes_received++; + + // Check to see if we are done receiving + if (m_bytes_received >= m_expected_size){ + m_file.flush(); + m_file.close(); + m_receiving = false; + m_data_file_count++; + + // Log data received event + Fw::LogStringArg logPath(m_currentFilename.c_str()); + this->log_ACTIVITY_HI_DataReceived(m_bytes_received, logPath); + } + } + } +} + +// ---------------------------------------------------------------------- +// Handler implementations for commands +// ---------------------------------------------------------------------- + +void PayloadHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) { + + // Append newline to command to send over UART + Fw::CmdStringArg tempCmd = cmd; + tempCmd += "\n"; + Fw::Buffer commandBuffer( + reinterpret_cast(const_cast(tempCmd.toChar())), + tempCmd.length() + ); + + // Send command over output port + Drv::ByteStreamStatus sendStatus = this->out_port_out(0, commandBuffer); + + Fw::LogStringArg logCmd(cmd); + + // Log success or failure + if (sendStatus != Drv::ByteStreamStatus::OP_OK) { + this->log_WARNING_HI_CommandError(logCmd); + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); + return; + } + else { + this->log_ACTIVITY_HI_CommandSuccess(logCmd); + } + + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp similarity index 79% rename from FprimeZephyrReference/Components/CameraManager/CameraManager.fpp rename to FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp index 955a21c4..0ca6356a 100644 --- a/FprimeZephyrReference/Components/CameraManager/CameraManager.fpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp @@ -1,18 +1,22 @@ module Components { @ Manager for Nicla Vision - passive component CameraManager { + passive component PayloadHandler { # One async command/port is required for active components # This should be overridden by the developers with a useful command/port @ TODO - sync command TAKE_IMAGE + sync command SEND_COMMAND(cmd: string) # Command to send data over UART - event TakeImageError() severity warning high format "Failed to take picture" + event CommandError(cmd: string) severity warning high format "Failed to send {} command over UART" - event PictureTaken() severity activity high format "Picture Taken" + event CommandSuccess(cmd: string) severity activity high format "Command {} sent successfully" + + event DataReceived( data: U8, path: string) severity activity high format "Stored {} bytes of payload data to {}" output port out_port: Drv.ByteStreamSend + sync input port in_port: Drv.ByteStreamData + ############################################################################## #### Uncomment the following examples to start customizing your component #### ############################################################################## diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp new file mode 100644 index 00000000..6897f750 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp @@ -0,0 +1,66 @@ +// ====================================================================== +// \title PayloadHandler.hpp +// \author robertpendergrast +// \brief hpp file for PayloadHandler component implementation class +// ====================================================================== + +#ifndef FprimeZephyrReference_PayloadHandler_HPP +#define FprimeZephyrReference_PayloadHandler_HPP + +#include +#include +#include "Os/File.hpp" +#include "FprimeZephyrReference/Components/PayloadHandler/PayloadHandlerComponentAc.hpp" + +namespace Components { + +class PayloadHandler final : public PayloadHandlerComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct PayloadHandler object + PayloadHandler(const char* const compName //!< The component name + ); + + //! Destroy PayloadHandler object + ~PayloadHandler(); + + U8 m_data_file_count = 0; + bool m_receiving = false; + U32 m_expected_size = 0; + U32 m_bytes_received = 0; + + U8 m_lineBuffer[128]; + size_t m_lineIndex = 0; + Os::File m_file; + std::string m_currentFilename; + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for in_port + //! Handler implementation for in_port + void in_port_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& buffer, + const Drv::ByteStreamStatus& status); + + private: + // ---------------------------------------------------------------------- + // Handler implementations for commands + // ---------------------------------------------------------------------- + + //! Handler implementation for command SEND_COMMAND + //! + //! TODO + void SEND_COMMAND_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq, //!< The command sequence number + const Fw::CmdStringArg& cmd); +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/Components/CameraManager/docs/sdd.md b/FprimeZephyrReference/Components/PayloadHandler/docs/sdd.md similarity index 100% rename from FprimeZephyrReference/Components/CameraManager/docs/sdd.md rename to FprimeZephyrReference/Components/PayloadHandler/docs/sdd.md diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index f00ad64c..97f2695b 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -90,7 +90,7 @@ module ReferenceDeployment { instance fsSpace: Components.FsSpace base id 0x10030000 - instance camera: Components.CameraManager base id 0x10031000 + instance payload: Components.PayloadHandler base id 0x10031000 instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x10032000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 4b6d2131..791c3089 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -44,7 +44,7 @@ module ReferenceDeployment { # For UART sideband communication instance comDriver instance fsSpace - instance camera + instance payload instance peripheralUartDriver @@ -164,8 +164,9 @@ module ReferenceDeployment { imuManager.temperatureGet -> lsm6dsoManager.temperatureGet } - connections Camera { - camera.out_port -> peripheralUartDriver.$send + connections PayloadHandler { + payload.out_port -> peripheralUartDriver.$send + peripheralUartDriver.$recv -> payload.in_port } } } 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 65637823..ac0b4ee0 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 @@ -21,6 +21,16 @@ 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 2af1cbbf..21d5c6dc 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 @@ -107,6 +107,13 @@ zephyr_udc0: &usbd { 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>; From 6162dc32736ea43ceda0a6af9304fc56d8fb2311 Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Mon, 3 Nov 2025 16:33:32 -0500 Subject: [PATCH 04/34] broke --- .../Components/PayloadHandler/PayloadHandler.cpp | 6 +++++- .../Components/PayloadHandler/PayloadHandler.fpp | 6 ++++++ FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp | 1 + 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index 1930bd29..2f0224ff 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -23,6 +23,8 @@ PayloadHandler ::~PayloadHandler() {} void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { + this->log_ACTIVITY_LO_UartReceived(); + const U8* data = buffer.getData(); FwSizeType size = buffer.getSize(); @@ -38,7 +40,7 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c if (m_lineIndex < sizeof(m_lineBuffer) - 1) { m_lineBuffer[m_lineIndex++] = byte; } - + this->log_ACTIVITY_LO_ByteReceived(byte); // Have we reached the end of the line? If so that means we have a header // Check to see what the header is. if (byte == '\n' || byte == '\r') { @@ -48,6 +50,7 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c // Check the header. // Right now I'm just checking for an image start tag, but we can expand this to other types later if (strstr((const char*)m_lineBuffer, "")) { + this->log_ACTIVITY_LO_ImageHeaderReceived(); m_receiving = true; m_bytes_received = 0; m_expected_size = 0; @@ -88,6 +91,7 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c // Write a byte to the file FwSizeType oneByte = 1; m_file.write(&byte, oneByte); + this->log_ACTIVITY_LO_ByteReceived(byte); m_bytes_received++; // Check to see if we are done receiving diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp index 0ca6356a..ce02dc95 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp @@ -13,6 +13,12 @@ module Components { event DataReceived( data: U8, path: string) severity activity high format "Stored {} bytes of payload data to {}" + event ByteReceived( byte: U8) severity activity low format "Received byte: {}" + + event ImageHeaderReceived() severity activity low format "Received image header" + + event UartReceived() severity activity low format "Received UART data" + output port out_port: Drv.ByteStreamSend sync input port in_port: Drv.ByteStreamData diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 791c3089..943a545f 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -126,6 +126,7 @@ module ReferenceDeployment { rateGroup10Hz.RateGroupMemberOut[0] -> comDriver.schedIn rateGroup10Hz.RateGroupMemberOut[1] -> ComCcsdsUart.aggregator.timeout rateGroup10Hz.RateGroupMemberOut[2] -> ComCcsds.aggregator.timeout + #rateGroup10Hz.RateGroupMemberOut[3] -> peripheralUartDriver.schedIn # Slow rate (1Hz) rate group rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1Hz] -> rateGroup1Hz.CycleIn From d671a93e8a22b4aad47f76c6e51a734bff80f3b8 Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Tue, 4 Nov 2025 19:05:55 -0500 Subject: [PATCH 05/34] Topologies --- .../PayloadHandler/PayloadHandler.cpp | 83 +------------------ .../ReferenceDeployment/Top/instances.fpp | 1 - .../ReferenceDeployment/Top/topology.fpp | 2 +- 3 files changed, 2 insertions(+), 84 deletions(-) diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index 2f0224ff..f9d85183 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -25,88 +25,7 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c this->log_ACTIVITY_LO_UartReceived(); - const U8* data = buffer.getData(); - FwSizeType size = buffer.getSize(); - - for (FwSizeType i = 0; i < size; i++) { - // Process each byte of data as needed - U8 byte = data[i]; - - - if (!m_receiving){ - // We are not currently receiving a file - - // Append byte to line buffer: This is how we check the header to determine data type - if (m_lineIndex < sizeof(m_lineBuffer) - 1) { - m_lineBuffer[m_lineIndex++] = byte; - } - this->log_ACTIVITY_LO_ByteReceived(byte); - // Have we reached the end of the line? If so that means we have a header - // Check to see what the header is. - if (byte == '\n' || byte == '\r') { - m_lineBuffer[m_lineIndex] = 0; // Null-terminate - m_lineIndex = 0; - - // Check the header. - // Right now I'm just checking for an image start tag, but we can expand this to other types later - if (strstr((const char*)m_lineBuffer, "")) { - this->log_ACTIVITY_LO_ImageHeaderReceived(); - m_receiving = true; - m_bytes_received = 0; - m_expected_size = 0; - continue; - } - - // If in receiving mode and expected size not set, this line is the size - if (m_receiving && m_expected_size == 0) { - - // First we set the expected size - m_expected_size = atoi((const char*)m_lineBuffer); - - - // Then we open the file to write to, which we will be writing to over a lot of iterations - if (m_data_file_count >= 9) { - m_data_file_count = 0; - } - - char filenameBuffer[20]; - snprintf(filenameBuffer, sizeof(filenameBuffer), "payload_%d.jpg", m_data_file_count); - m_currentFilename = filenameBuffer; - - // Open the file and prepare to write in the next iteration - Os::File::Status fileStatus = m_file.open(m_currentFilename.c_str(), Os::File::OPEN_CREATE, Os::File::OVERWRITE); - if (fileStatus != Os::File::OP_OK) { - m_receiving = false; - continue; - } - continue; - } - } - - - } else if (m_bytes_received < m_expected_size){ - // We are currently receiving a file - - // Cast byte to a buffer - // Write a byte to the file - FwSizeType oneByte = 1; - m_file.write(&byte, oneByte); - this->log_ACTIVITY_LO_ByteReceived(byte); - m_bytes_received++; - - // Check to see if we are done receiving - if (m_bytes_received >= m_expected_size){ - m_file.flush(); - m_file.close(); - m_receiving = false; - m_data_file_count++; - - // Log data received event - Fw::LogStringArg logPath(m_currentFilename.c_str()); - this->log_ACTIVITY_HI_DataReceived(m_bytes_received, logPath); - } - } - } + } // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 97f2695b..2177ebf3 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -94,5 +94,4 @@ module ReferenceDeployment { instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x10032000 - } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 943a545f..a6ef8e89 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -126,7 +126,7 @@ module ReferenceDeployment { rateGroup10Hz.RateGroupMemberOut[0] -> comDriver.schedIn rateGroup10Hz.RateGroupMemberOut[1] -> ComCcsdsUart.aggregator.timeout rateGroup10Hz.RateGroupMemberOut[2] -> ComCcsds.aggregator.timeout - #rateGroup10Hz.RateGroupMemberOut[3] -> peripheralUartDriver.schedIn + rateGroup10Hz.RateGroupMemberOut[3] -> peripheralUartDriver.schedIn # Slow rate (1Hz) rate group rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1Hz] -> rateGroup1Hz.CycleIn From fb1edfb974c32d058f6167a032721e1bda4addb9 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Wed, 5 Nov 2025 20:31:42 -0500 Subject: [PATCH 06/34] builds (?) --- .../PayloadHandler/PayloadHandler.cpp | 301 ++++++++++++++++- .../PayloadHandler/PayloadHandler.fpp | 14 +- .../PayloadHandler/PayloadHandler.hpp | 47 ++- .../PayloadHandler/docs/BUFFER_USAGE.md | 239 ++++++++++++++ .../Components/PayloadHandler/docs/USAGE.md | 306 ++++++++++++++++++ .../Top/ReferenceDeploymentPackets.fppi | 5 + .../ReferenceDeployment/Top/instances.fpp | 22 ++ .../ReferenceDeployment/Top/topology.fpp | 6 + .../project/config/AcConstants.fpp | 32 ++ .../project/config/CMakeLists.txt | 1 + 10 files changed, 967 insertions(+), 6 deletions(-) create mode 100644 FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md create mode 100644 FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md create mode 100644 FprimeZephyrReference/project/config/AcConstants.fpp diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index f9d85183..a18df09b 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -4,7 +4,10 @@ // \brief cpp file for PayloadHandler component implementation class // ====================================================================== #include "Os/File.hpp" +#include "Fw/Types/Assert.hpp" +#include "Fw/Types/BasicTypes.hpp" #include "FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp" +#include namespace Components { @@ -12,8 +15,18 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -PayloadHandler ::PayloadHandler(const char* const compName) : PayloadHandlerComponentBase(compName) {} -PayloadHandler ::~PayloadHandler() {} +PayloadHandler ::PayloadHandler(const char* const compName) + : PayloadHandlerComponentBase(compName), + m_protocolBufferSize(0), + m_imageBufferUsed(0) { + // Initialize protocol buffer to zero + memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); +} + +PayloadHandler ::~PayloadHandler() { + // Clean up any allocated image buffer + deallocateImageBuffer(); +} // ---------------------------------------------------------------------- @@ -25,7 +38,63 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c this->log_ACTIVITY_LO_UartReceived(); - + // Check if we received data successfully + if (status != Drv::ByteStreamStatus::OP_OK) { + // TODO - log error event? + return; + } + + // Check if buffer is valid + if (!buffer.isValid()) { + return; + } + + // Get the data from the incoming buffer + const U8* data = buffer.getData(); + const U32 dataSize = static_cast(buffer.getSize()); + + // Unclear if this works as intended if data flow is interrupted + + if (m_receiving && m_imageBuffer.isValid()) { + // Currently receiving image data - accumulate into large buffer + // Check for end marker before accumulating + I32 endMarkerPos = findImageEndMarker(data, dataSize); + + if (endMarkerPos >= 0) { + // Found end marker - accumulate data up to marker + U32 finalDataSize = static_cast(endMarkerPos); + if (finalDataSize > 0) { + if (!accumulateImageData(data, finalDataSize)) { + // Overflow + this->log_WARNING_HI_ImageDataOverflow(); + deallocateImageBuffer(); + m_receiving = false; + return; + } + } + + // Image is complete + processCompleteImage(); + } else { + // No end marker yet - accumulate all data + if (!accumulateImageData(data, dataSize)) { + // Image buffer overflow + this->log_WARNING_HI_ImageDataOverflow(); + deallocateImageBuffer(); + m_receiving = false; + } + } + } else { + // Not receiving image - accumulate protocol data + if (!accumulateProtocolData(data, dataSize)) { + // Protocol buffer overflow - clear and retry + clearProtocolBuffer(); + accumulateProtocolData(data, dataSize); + } + + // Process protocol buffer to detect image headers/commands + processProtocolBuffer(); + } } // ---------------------------------------------------------------------- @@ -60,4 +129,230 @@ void PayloadHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, c this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +// ---------------------------------------------------------------------- +// Helper method implementations +// ---------------------------------------------------------------------- + +bool PayloadHandler ::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 PayloadHandler ::processProtocolBuffer() { + // Process newline-terminated commands/headers + // Looking for "" to begin image reception + + while (m_protocolBufferSize > 0) { + // Look for newline character + bool foundNewline = false; + U32 lineEndIndex = 0; + + for (U32 i = 0; i < m_protocolBufferSize; ++i) { + if (m_protocolBuffer[i] == '\n' || m_protocolBuffer[i] == '\r') { + foundNewline = true; + lineEndIndex = i; + break; + } + } + + if (foundNewline) { + U32 lineLength = lineEndIndex; + + // Skip carriage return if present (handle \r\n) + // needed? + if (lineEndIndex + 1 < m_protocolBufferSize && + m_protocolBuffer[lineEndIndex] == '\r' && + m_protocolBuffer[lineEndIndex + 1] == '\n') { + lineEndIndex++; + } + + // Check if this is the image start command + if (isImageStartCommand(m_protocolBuffer, lineLength)) { + // Allocate buffer for image data + if (allocateImageBuffer()) { + m_receiving = true; + m_bytes_received = 0; + + // Generate filename + char filename[64]; + snprintf(filename, sizeof(filename), "/mnt/data/img_%03d.jpg", m_data_file_count++); + m_currentFilename = filename; + + this->log_ACTIVITY_LO_ImageHeaderReceived(); + + // Remove the IMG_START line from buffer + U32 remainingSize = m_protocolBufferSize - (lineEndIndex + 1); + if (remainingSize > 0) { + memmove(m_protocolBuffer, + &m_protocolBuffer[lineEndIndex + 1], + remainingSize); + } + m_protocolBufferSize = remainingSize; + + // Any remaining data in protocol buffer is image data, so we transfer it to the image buffer immediately + if (m_protocolBufferSize > 0) { + if (!accumulateImageData(m_protocolBuffer, m_protocolBufferSize)) { + this->log_WARNING_HI_ImageDataOverflow(); + deallocateImageBuffer(); + m_receiving = false; + } + clearProtocolBuffer(); // Clear now that data is moved + } + + // Exit loop - we're now in image receiving mode + break; + } else { + // Buffer allocation failed + this->log_WARNING_HI_BufferAllocationFailed(IMAGE_BUFFER_SIZE); + } + } else { + // Log other commands/data for debugging + for (U32 i = 0; i < lineLength && i < 16; ++i) { + this->log_ACTIVITY_LO_ByteReceived(m_protocolBuffer[i]); + } + + // Remove processed line from buffer + U32 remainingSize = m_protocolBufferSize - (lineEndIndex + 1); + if (remainingSize > 0) { + memmove(m_protocolBuffer, + &m_protocolBuffer[lineEndIndex + 1], + remainingSize); + } + m_protocolBufferSize = remainingSize; + } + } else { + break; + } + } +} + +void PayloadHandler ::clearProtocolBuffer() { + m_protocolBufferSize = 0; + memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); +} + +bool PayloadHandler ::allocateImageBuffer() { + // Request buffer from BufferManager + m_imageBuffer = this->allocate_out(0, IMAGE_BUFFER_SIZE); + + // Check if allocation succeeded + if (!m_imageBuffer.isValid() || m_imageBuffer.getSize() < IMAGE_BUFFER_SIZE) { + this->log_WARNING_HI_BufferAllocationFailed(IMAGE_BUFFER_SIZE); + deallocateImageBuffer(); + return false; + } + + m_imageBufferUsed = 0; + return true; +} + +void PayloadHandler ::deallocateImageBuffer() { + if (m_imageBuffer.isValid()) { + this->deallocate_out(0, m_imageBuffer); + m_imageBuffer = Fw::Buffer(); // Reset to invalid buffer + } + m_imageBufferUsed = 0; +} + +bool PayloadHandler ::accumulateImageData(const U8* data, U32 size) { + FW_ASSERT(m_imageBuffer.isValid()); + + // Check if we have space + if (m_imageBufferUsed + size > m_imageBuffer.getSize()) { + return false; + } + + // Copy data into image buffer + memcpy(&m_imageBuffer.getData()[m_imageBufferUsed], data, size); + m_imageBufferUsed += size; + m_bytes_received += size; + + return true; +} + +void PayloadHandler ::processCompleteImage() { + FW_ASSERT(m_imageBuffer.isValid()); + + // Write image to file + Os::File::Status status = m_file.open(m_currentFilename.c_str(), Os::File::OPEN_WRITE); + + if (status == Os::File::OP_OK) { + // Os::File::write expects FwSizeType& for size parameter + FwSizeType sizeToWrite = static_cast(m_imageBufferUsed); + status = m_file.write(m_imageBuffer.getData(), sizeToWrite, Os::File::WaitType::NO_WAIT); + m_file.close(); + + if (status == Os::File::OP_OK) { + // Success! sizeToWrite now contains actual bytes written + Fw::LogStringArg pathArg(m_currentFilename.c_str()); + this->log_ACTIVITY_HI_DataReceived(m_imageBufferUsed, pathArg); + } else { + // TODO - log write error + } + } else { + // TODO - log open error + } + + // Clean up + deallocateImageBuffer(); + m_receiving = false; + m_bytes_received = 0; +} + +I32 PayloadHandler ::findImageEndMarker(const U8* data, U32 size) { + // Looking for "\n" or "" + const char* marker = ""; + const U32 markerLen = 9; // strlen("") + + if (size < markerLen) { + return -1; + } + + // Search for the marker + for (U32 i = 0; i <= size - markerLen; ++i) { + bool found = true; + for (U32 j = 0; j < markerLen; ++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 PayloadHandler ::isImageStartCommand(const U8* line, U32 length) { + const char* command = ""; + const U32 cmdLen = 11; // strlen("") + + if (length != cmdLen) { + return false; + } + + for (U32 i = 0; i < cmdLen; ++i) { + if (line[i] != static_cast(command[i])) { + return false; + } + } + + return true; +} + } // namespace Components diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp index ce02dc95..1d28fa19 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp @@ -19,9 +19,19 @@ module Components { event UartReceived() severity activity low format "Received UART data" + event BufferAllocationFailed(buffer_size: U32) severity warning high format "Failed to allocate buffer of size {}" + + event ImageDataOverflow() severity warning high format "Image data overflow - buffer full" + output port out_port: Drv.ByteStreamSend - sync input port in_port: Drv.ByteStreamData + sync input port in_port: Drv.ByteStreamData + + @ Port for allocating buffers for image data + output port allocate: Fw.BufferGet + + @ Port for deallocating buffers + output port deallocate: Fw.BufferSend ############################################################################## #### Uncomment the following examples to start customizing your component #### @@ -73,4 +83,4 @@ module Components { param set port prmSetOut } -} \ No newline at end of file +} diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp index 6897f750..11e37ee4 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp @@ -29,7 +29,6 @@ class PayloadHandler final : public PayloadHandlerComponentBase { U8 m_data_file_count = 0; bool m_receiving = false; - U32 m_expected_size = 0; U32 m_bytes_received = 0; U8 m_lineBuffer[128]; @@ -37,6 +36,16 @@ class PayloadHandler final : public PayloadHandlerComponentBase { Os::File m_file; std::string m_currentFilename; + // Small protocol buffer for commands/headers (static allocation) + static constexpr U32 PROTOCOL_BUFFER_SIZE = 2048; + U8 m_protocolBuffer[PROTOCOL_BUFFER_SIZE]; + U32 m_protocolBufferSize = 0; + + // Large image buffer (dynamic allocation via BufferManager) + Fw::Buffer m_imageBuffer; + U32 m_imageBufferUsed = 0; // Bytes used in image buffer + static constexpr U32 IMAGE_BUFFER_SIZE = 256 * 1024; // 256 KB for images + private: // ---------------------------------------------------------------------- // Handler implementations for typed input ports @@ -59,6 +68,42 @@ class PayloadHandler final : public PayloadHandlerComponentBase { void SEND_COMMAND_cmdHandler(FwOpcodeType opCode, //!< The opcode U32 cmdSeq, //!< The command sequence number const Fw::CmdStringArg& cmd); + + // ---------------------------------------------------------------------- + // Helper methods for data accumulation + // ---------------------------------------------------------------------- + + //! 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(); + + //! Allocate image buffer from BufferManager + //! Returns true on success + bool allocateImageBuffer(); + + //! Deallocate image buffer + void deallocateImageBuffer(); + + //! Accumulate image data into dynamically allocated buffer + //! Returns true on success, false on overflow + bool accumulateImageData(const U8* data, U32 size); + + //! Process complete image (write to file, send event, etc.) + void processCompleteImage(); + + //! 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); }; } // namespace Components diff --git a/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md b/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md new file mode 100644 index 00000000..38729b2e --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md @@ -0,0 +1,239 @@ +# PayloadHandler Buffer Strategy + +## Overview + +The PayloadHandler 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) + ↓ + PayloadHandler + ↓ + ┌─────┴─────┐ + ↓ ↓ +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 "\n" → allocateImageBuffer() + → m_receiving = true + → Start accumulating +``` + +### 3. Receiving Image +``` +Incoming data → Image Buffer (accumulate) → Check each chunk for "" +``` + +### 4. Image Complete +``` +Detect "\n" → processCompleteImage() → Write to file + → deallocateImageBuffer() + → m_receiving = false +``` + +## Camera Protocol + +The Nicla Vision camera sends images using this protocol: + +``` +\n +[raw JPEG data in 512-byte chunks] +\n\n +``` + +Example: +``` + +ÿØÿà...JFIF...image data...ÿÙ + +``` + +## 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** (PayloadHandler.hpp): +```cpp +static constexpr U32 PROTOCOL_BUFFER_SIZE = 2048; // Change this +``` + +2. **Image buffer** (PayloadHandler.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. Scans for newline-terminated lines +2. Checks if line matches `` +3. If match, calls `allocateImageBuffer()` and sets `m_receiving = true` +4. Generates filename: `/mnt/data/img_NNN.jpg` + +### Image Reception (Implemented) + +The `in_port_handler()` function: +- When `m_receiving == true`, accumulates data into image buffer +- Each chunk is checked for `` marker using `findImageEndMarker()` +- When marker found, extracts image data (before marker) and calls `processCompleteImage()` + +### End Marker Detection + +The camera sends: +``` +[image data]\n\n +``` + +The handler searches each incoming chunk for `` and stops accumulation when found. Image data before the marker is written to file. + +## 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/PayloadHandler/docs/USAGE.md b/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md new file mode 100644 index 00000000..617e3843 --- /dev/null +++ b/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md @@ -0,0 +1,306 @@ +# PayloadHandler Usage Guide + +## Overview + +The PayloadHandler 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: + ``` + + [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** + - PayloadHandler receives `\n` + - 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 `\n` marker + - 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 + +### 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 `` 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 PayloadHandler.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: "\n" + [JPEG file bytes] + "\n\n" +``` + +### 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 `PayloadHandler.hpp`: +```cpp +static constexpr U32 IMAGE_BUFFER_SIZE = 512 * 1024; +``` + +### Adding Image Size to Protocol + +For better error handling, modify camera code to send size: + +```python +# Camera side +size = uos.stat(filename)[6] # Get file size +uart.write(f"\n".encode()) +``` + +Then parse in `isImageStartCommand()` to set `m_expected_size`. + +### 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/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 43f0bbf9..03d20d82 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -17,6 +17,8 @@ telemetry packets ReferenceDeploymentPackets { ComCcsds.commsBufferManager.EmptyBuffs ComCcsdsUart.commsBufferManager.NoBuffs ComCcsdsUart.commsBufferManager.EmptyBuffs + payloadBufferManager.NoBuffs + payloadBufferManager.EmptyBuffs ReferenceDeployment.rateGroup10Hz.RgCycleSlips ReferenceDeployment.rateGroup1Hz.RgCycleSlips } @@ -29,6 +31,9 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.TotalBuffs ComCcsdsUart.commsBufferManager.CurrBuffs ComCcsdsUart.comQueue.buffQueueDepth + payloadBufferManager.TotalBuffs + payloadBufferManager.CurrBuffs + payloadBufferManager.HiBuffs CdhCore.tlmSend.SendLevel } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 2177ebf3..668b755a 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -94,4 +94,26 @@ module ReferenceDeployment { instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x10032000 + instance payloadBufferManager: Svc.BufferManager base id 0x10033000 \ + { + 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)); + // Configure for 256 KB image buffers, 2 buffers + ConfigObjects::ReferenceDeployment_payloadBufferManager::bins.bins[0].bufferSize = 256 * 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 a6ef8e89..27573831 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -46,6 +46,7 @@ module ReferenceDeployment { instance fsSpace instance payload instance peripheralUartDriver + instance payloadBufferManager # ---------------------------------------------------------------------- @@ -140,6 +141,7 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[7] -> burnwire.schedIn rateGroup1Hz.RateGroupMemberOut[8] -> antennaDeployer.schedIn rateGroup1Hz.RateGroupMemberOut[9] -> fsSpace.run + rateGroup1Hz.RateGroupMemberOut[10] -> payloadBufferManager.schedIn } @@ -168,6 +170,10 @@ module ReferenceDeployment { connections PayloadHandler { payload.out_port -> peripheralUartDriver.$send peripheralUartDriver.$recv -> payload.in_port + + # Buffer management for image data + payload.allocate -> payloadBufferManager.bufferGetCallee + payload.deallocate -> payloadBufferManager.bufferSendIn } } } diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp new file mode 100644 index 00000000..c90c5e15 --- /dev/null +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -0,0 +1,32 @@ +# ====================================================================== +# AcConstants.fpp +# F Prime configuration constants (project override) +# ====================================================================== + +# Override: Increase from default 10 to accommodate all 1Hz rate group members +constant ActiveRateGroupOutputPorts = 15 + +# Defaults +constant PassiveRateGroupOutputPorts = 10 +constant RateGroupDriverRateGroupPorts = 3 +constant CmdDispatcherComponentCommandPorts = 30 +constant CmdDispatcherSequencePorts = 5 +constant SeqDispatcherSequencerPorts = 2 +constant CmdSplitterPorts = CmdDispatcherSequencePorts +constant StaticMemoryAllocations = 4 +constant HealthPingPorts = 25 +constant FileDownCompletePorts = 1 +constant ComQueueComPorts = 2 +constant ComQueueBufferPorts = 1 +constant BufferRepeaterOutputPorts = 10 +constant DpManagerNumPorts = 5 +constant DpWriterNumProcPorts = 5 +constant FileNameStringSize = 200 +constant FwAssertTextSize = 120 +constant AssertFatalAdapterEventFileSize = FileNameStringSize + +# Hub connections +constant GenericHubInputPorts = 10 +constant GenericHubOutputPorts = 10 +constant GenericHubInputBuffers = 10 +constant GenericHubOutputBuffers = 10 diff --git a/FprimeZephyrReference/project/config/CMakeLists.txt b/FprimeZephyrReference/project/config/CMakeLists.txt index cf115d7b..0e62ab4e 100644 --- a/FprimeZephyrReference/project/config/CMakeLists.txt +++ b/FprimeZephyrReference/project/config/CMakeLists.txt @@ -5,6 +5,7 @@ #### register_fprime_config( CONFIGURATION_OVERRIDES + "${CMAKE_CURRENT_LIST_DIR}/AcConstants.fpp" "${CMAKE_CURRENT_LIST_DIR}/CdhCoreConfig.fpp" "${CMAKE_CURRENT_LIST_DIR}/CdhCoreTlmConfig.fpp" "${CMAKE_CURRENT_LIST_DIR}/CdhCoreFatalHandlerConfig.fpp" From 461ebb7da126ee28d7b39afae0e89eeab338e812 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Wed, 5 Nov 2025 21:15:46 -0500 Subject: [PATCH 07/34] wraparound footer detection --- .../PayloadHandler/PayloadHandler.cpp | 52 +++++++++++-------- 1 file changed, 31 insertions(+), 21 deletions(-) diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index a18df09b..7f2c8f19 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -57,32 +57,42 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c if (m_receiving && m_imageBuffer.isValid()) { // Currently receiving image data - accumulate into large buffer - // Check for end marker before accumulating - I32 endMarkerPos = findImageEndMarker(data, dataSize); - if (endMarkerPos >= 0) { - // Found end marker - accumulate data up to marker - U32 finalDataSize = static_cast(endMarkerPos); - if (finalDataSize > 0) { - if (!accumulateImageData(data, finalDataSize)) { - // Overflow - this->log_WARNING_HI_ImageDataOverflow(); - deallocateImageBuffer(); - m_receiving = false; - return; - } + // First accumulate the new data + if (!accumulateImageData(data, dataSize)) { + // Image buffer overflow + this->log_WARNING_HI_ImageDataOverflow(); + deallocateImageBuffer(); + m_receiving = false; + return; + } + + // Now check if end marker is in the accumulated buffer + // We search the last portion (where marker could be) + const U32 markerLen = 9; // strlen("") + U32 searchStart = 0; + if (m_imageBufferUsed > markerLen) { + // Start search from near the end to handle split markers + searchStart = m_imageBufferUsed - dataSize - markerLen; + if (searchStart > m_imageBufferUsed) { + searchStart = 0; // Wraparound protection } + } + + I32 endMarkerPos = findImageEndMarker( + &m_imageBuffer.getData()[searchStart], + m_imageBufferUsed - searchStart + ); + + if (endMarkerPos >= 0) { + // Found end marker - adjust position to absolute offset + U32 absolutePos = searchStart + static_cast(endMarkerPos); + + // Trim the image buffer to remove the end marker + m_imageBufferUsed = absolutePos; // Image is complete processCompleteImage(); - } else { - // No end marker yet - accumulate all data - if (!accumulateImageData(data, dataSize)) { - // Image buffer overflow - this->log_WARNING_HI_ImageDataOverflow(); - deallocateImageBuffer(); - m_receiving = false; - } } } else { // Not receiving image - accumulate protocol data From dc5b934b6b9d76d19cd833d6346707b429dae086 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Thu, 6 Nov 2025 14:27:11 -0500 Subject: [PATCH 08/34] new camera protocol --- .../PayloadHandler/PayloadHandler.cpp | 184 ++++++++---------- .../PayloadHandler/PayloadHandler.hpp | 16 ++ .../PayloadHandler/docs/BUFFER_USAGE.md | 58 +++--- .../Components/PayloadHandler/docs/USAGE.md | 45 +++-- 4 files changed, 159 insertions(+), 144 deletions(-) diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index 7f2c8f19..575306d5 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -8,6 +8,7 @@ #include "Fw/Types/BasicTypes.hpp" #include "FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp" #include +#include namespace Components { @@ -67,29 +68,10 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c return; } - // Now check if end marker is in the accumulated buffer - // We search the last portion (where marker could be) - const U32 markerLen = 9; // strlen("") - U32 searchStart = 0; - if (m_imageBufferUsed > markerLen) { - // Start search from near the end to handle split markers - searchStart = m_imageBufferUsed - dataSize - markerLen; - if (searchStart > m_imageBufferUsed) { - searchStart = 0; // Wraparound protection - } - } - - I32 endMarkerPos = findImageEndMarker( - &m_imageBuffer.getData()[searchStart], - m_imageBufferUsed - searchStart - ); - - if (endMarkerPos >= 0) { - // Found end marker - adjust position to absolute offset - U32 absolutePos = searchStart + static_cast(endMarkerPos); - - // Trim the image buffer to remove the end marker - m_imageBufferUsed = absolutePos; + // Check if we've received all expected data + if (m_expected_size > 0 && m_imageBufferUsed >= m_expected_size) { + // We have all the data! Trim to exact size (in case we got too) + m_imageBufferUsed = m_expected_size; // Image is complete processCompleteImage(); @@ -157,89 +139,85 @@ bool PayloadHandler ::accumulateProtocolData(const U8* data, U32 size) { } void PayloadHandler ::processProtocolBuffer() { - // Process newline-terminated commands/headers - // Looking for "" to begin image reception + // Protocol: [4-byte little-endian uint32][image data] - while (m_protocolBufferSize > 0) { - // Look for newline character - bool foundNewline = false; - U32 lineEndIndex = 0; - - for (U32 i = 0; i < m_protocolBufferSize; ++i) { - if (m_protocolBuffer[i] == '\n' || m_protocolBuffer[i] == '\r') { - foundNewline = true; - lineEndIndex = i; + 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 (foundNewline) { - U32 lineLength = lineEndIndex; + + 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! Allocate buffer + if (allocateImageBuffer()) { + m_receiving = true; + m_bytes_received = 0; + m_expected_size = imageSize; - // Skip carriage return if present (handle \r\n) - // needed? - if (lineEndIndex + 1 < m_protocolBufferSize && - m_protocolBuffer[lineEndIndex] == '\r' && - m_protocolBuffer[lineEndIndex + 1] == '\n') { - lineEndIndex++; + // Generate filename + char filename[64]; + snprintf(filename, sizeof(filename), "/mnt/data/img_%03d.jpg", m_data_file_count++); + m_currentFilename = filename; + + this->log_ACTIVITY_LO_ImageHeaderReceived(); + + // Remove header + U32 remainingSize = m_protocolBufferSize - HEADER_SIZE; + if (remainingSize > 0) { + memmove(m_protocolBuffer, + &m_protocolBuffer[HEADER_SIZE], + remainingSize); } - - // Check if this is the image start command - if (isImageStartCommand(m_protocolBuffer, lineLength)) { - // Allocate buffer for image data - if (allocateImageBuffer()) { - m_receiving = true; - m_bytes_received = 0; - - // Generate filename - char filename[64]; - snprintf(filename, sizeof(filename), "/mnt/data/img_%03d.jpg", m_data_file_count++); - m_currentFilename = filename; - - this->log_ACTIVITY_LO_ImageHeaderReceived(); - - // Remove the IMG_START line from buffer - U32 remainingSize = m_protocolBufferSize - (lineEndIndex + 1); - if (remainingSize > 0) { - memmove(m_protocolBuffer, - &m_protocolBuffer[lineEndIndex + 1], - remainingSize); - } - m_protocolBufferSize = remainingSize; - - // Any remaining data in protocol buffer is image data, so we transfer it to the image buffer immediately - if (m_protocolBufferSize > 0) { - if (!accumulateImageData(m_protocolBuffer, m_protocolBufferSize)) { - this->log_WARNING_HI_ImageDataOverflow(); - deallocateImageBuffer(); - m_receiving = false; - } - clearProtocolBuffer(); // Clear now that data is moved - } - - // Exit loop - we're now in image receiving mode - break; - } else { - // Buffer allocation failed - this->log_WARNING_HI_BufferAllocationFailed(IMAGE_BUFFER_SIZE); - } - } else { - // Log other commands/data for debugging - for (U32 i = 0; i < lineLength && i < 16; ++i) { - this->log_ACTIVITY_LO_ByteReceived(m_protocolBuffer[i]); - } - - // Remove processed line from buffer - U32 remainingSize = m_protocolBufferSize - (lineEndIndex + 1); - if (remainingSize > 0) { - memmove(m_protocolBuffer, - &m_protocolBuffer[lineEndIndex + 1], - remainingSize); + m_protocolBufferSize = remainingSize; + + // Transfer any remaining data (image data) to image buffer + if (m_protocolBufferSize > 0) { + if (!accumulateImageData(m_protocolBuffer, m_protocolBufferSize)) { + this->log_WARNING_HI_ImageDataOverflow(); + deallocateImageBuffer(); + m_receiving = false; } - m_protocolBufferSize = remainingSize; + clearProtocolBuffer(); } } else { - break; + // Buffer allocation failed + this->log_WARNING_HI_BufferAllocationFailed(IMAGE_BUFFER_SIZE); } } } @@ -320,16 +298,15 @@ void PayloadHandler ::processCompleteImage() { I32 PayloadHandler ::findImageEndMarker(const U8* data, U32 size) { // Looking for "\n" or "" const char* marker = ""; - const U32 markerLen = 9; // strlen("") - if (size < markerLen) { + if (size < IMG_END_LEN) { return -1; } // Search for the marker - for (U32 i = 0; i <= size - markerLen; ++i) { + for (U32 i = 0; i <= size - IMG_END_LEN; ++i) { bool found = true; - for (U32 j = 0; j < markerLen; ++j) { + for (U32 j = 0; j < IMG_END_LEN; ++j) { if (data[i + j] != static_cast(marker[j])) { found = false; break; @@ -350,13 +327,12 @@ I32 PayloadHandler ::findImageEndMarker(const U8* data, U32 size) { bool PayloadHandler ::isImageStartCommand(const U8* line, U32 length) { const char* command = ""; - const U32 cmdLen = 11; // strlen("") - if (length != cmdLen) { + if (length < IMG_START_LEN) { return false; } - for (U32 i = 0; i < cmdLen; ++i) { + for (U32 i = 0; i < IMG_START_LEN; ++i) { if (line[i] != static_cast(command[i])) { return false; } diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp index 11e37ee4..caba8e1d 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp @@ -45,6 +45,22 @@ class PayloadHandler final : public PayloadHandlerComponentBase { Fw::Buffer m_imageBuffer; U32 m_imageBufferUsed = 0; // Bytes used in image buffer static constexpr U32 IMAGE_BUFFER_SIZE = 256 * 1024; // 256 KB for images + + // 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("") + + // 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 private: // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md b/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md index 38729b2e..84c1e959 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md +++ b/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md @@ -58,21 +58,22 @@ Incoming data → Protocol Buffer → Parse for commands ### 2. Image Header Detected ``` -Parse "\n" → allocateImageBuffer() - → m_receiving = true - → Start accumulating +Parse "...bytes..." → Extract image size (m_expected_size) + → allocateImageBuffer() + → m_receiving = true + → Start accumulating ``` ### 3. Receiving Image ``` -Incoming data → Image Buffer (accumulate) → Check each chunk for "" +Incoming data → Image Buffer (accumulate) → Check if m_imageBufferUsed >= m_expected_size ``` ### 4. Image Complete ``` -Detect "\n" → processCompleteImage() → Write to file - → deallocateImageBuffer() - → m_receiving = false +Received m_expected_size bytes → processCompleteImage() → Write to file + → deallocateImageBuffer() + → m_receiving = false ``` ## Camera Protocol @@ -80,18 +81,20 @@ Detect "\n" → processCompleteImage() → Write to file The Nicla Vision camera sends images using this protocol: ``` -\n -[raw JPEG data in 512-byte chunks] -\n\n +[4-byte little-endian uint32][raw JPEG data in 512-byte chunks] ``` Example: ``` - -ÿØÿà...JFIF...image data...ÿÙ - +\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 @@ -184,26 +187,33 @@ BufferManager will allocate the smallest buffer that fits the request. ### Protocol Parsing (Implemented) The `processProtocolBuffer()` function: -1. Scans for newline-terminated lines -2. Checks if line matches `` -3. If match, calls `allocateImageBuffer()` and sets `m_receiving = true` -4. Generates filename: `/mnt/data/img_NNN.jpg` +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 -- Each chunk is checked for `` marker using `findImageEndMarker()` -- When marker found, extracts image data (before marker) and calls `processCompleteImage()` +- After each chunk, checks if `m_imageBufferUsed >= m_expected_size` +- When complete, trims to exact size and calls `processCompleteImage()` -### End Marker Detection +### Size-Based Completion -The camera sends: -``` -[image data]\n\n +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(); +} ``` -The handler searches each incoming chunk for `` and stops accumulation when found. Image data before the marker is written to file. +No need to search for end markers in every chunk! ## Error Handling diff --git a/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md b/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md index 617e3843..f971cf75 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md +++ b/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md @@ -18,9 +18,7 @@ The camera will: 2. Save it locally as `images/img_NNNN.jpg` 3. Send it back over UART with the protocol: ``` - - [JPEG data] - + [4-byte size][JPEG data] ``` ## F' Commands @@ -43,7 +41,8 @@ This forwards the command over UART to the camera. - Event: `CommandSuccess` - "Command snap sent successfully" 2. **Image Start** - - PayloadHandler receives `\n` + - PayloadHandler receives `...bytes...` + - Parses image size from 4-byte little-endian value - Allocates 256 KB buffer from BufferManager - Event: `ImageHeaderReceived` - "Received image header" @@ -53,10 +52,11 @@ This forwards the command over UART to the camera. - Event: `UartReceived` - "Received UART data" (multiple times) 4. **Image Complete** - - Receives `\n` marker + - 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 @@ -66,7 +66,7 @@ Images are saved with sequential filenames: - `/mnt/data/img_002.jpg` - etc. -Counter increments each time `` is received. +Counter increments each time a valid `...` header is received. ## Error Cases @@ -150,9 +150,21 @@ TotalBuffs = 2 ``` Command from FS: "snap\n" -Camera response: "\n" - [JPEG file bytes] - "\n\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 @@ -276,17 +288,18 @@ Must also update `PayloadHandler.hpp`: static constexpr U32 IMAGE_BUFFER_SIZE = 512 * 1024; ``` -### Adding Image Size to Protocol +### Verifying Image Integrity -For better error handling, modify camera code to send size: +Since the protocol now includes size, you can verify completeness: -```python -# Camera side -size = uos.stat(filename)[6] # Get file size -uart.write(f"\n".encode()) +```cpp +// In processCompleteImage() +if (m_imageBufferUsed != m_expected_size) { + this->log_WARNING_HI_ImageSizeMismatch(m_expected_size, m_imageBufferUsed); +} ``` -Then parse in `isImageStartCommand()` to set `m_expected_size`. +This catches truncation or corruption during transmission. ### Multiple Image Formats From 23ed6f4441ac30908dc8c1e3d28d6435fcda4544 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Fri, 14 Nov 2025 18:54:21 -0500 Subject: [PATCH 09/34] Fix baseIds and Cmake after merge --- .../Components/CMakeLists.txt | 4 - .../ReferenceDeployment/Top/instances.fpp | 88 +++++++++---------- 2 files changed, 44 insertions(+), 48 deletions(-) diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index eb2933aa..d88d956c 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -14,8 +14,4 @@ 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}/Burnwire/") -add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/BootloaderTrigger/") -add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/AntennaDeployer/") -add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/FsSpace/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/PayloadHandler/") diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 2f19473f..de525d4c 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -37,12 +37,12 @@ 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 @@ -75,29 +75,45 @@ 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 payload: Components.PayloadHandler base id 0x10031000 + instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x10023000 - instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x10032000 + instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x10024000 - instance payloadBufferManager: Svc.BufferManager base id 0x10033000 \ + instance gpioface1LS: Zephyr.ZephyrGpioDriver base id 0x10025000 + + instance gpioface2LS: Zephyr.ZephyrGpioDriver base id 0x10026000 + + instance gpioface3LS: Zephyr.ZephyrGpioDriver base id 0x10027000 + + instance gpioface5LS: Zephyr.ZephyrGpioDriver base id 0x10028000 + + instance gpioPayloadPowerLS: Zephyr.ZephyrGpioDriver base id 0x10029000 + + instance gpioPayloadBatteryLS: Zephyr.ZephyrGpioDriver base id 0x1002A000 + + instance payload: Components.PayloadHandler base id 0x1002B000 + + instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x1002C000 + + instance payloadBufferManager: Svc.BufferManager base id 0x1002D000 \ { phase Fpp.ToCpp.Phases.configObjects """ Svc::BufferManager::BufferBins bins; @@ -118,48 +134,32 @@ module ReferenceDeployment { ReferenceDeployment::payloadBufferManager.cleanup(); """ } - instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x1002A000 - - instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x1002B000 - - instance gpioface1LS: Zephyr.ZephyrGpioDriver base id 0x1002C000 - - instance gpioface2LS: Zephyr.ZephyrGpioDriver base id 0x1002D000 - - instance gpioface3LS: Zephyr.ZephyrGpioDriver base id 0x1002E000 - - instance gpioface5LS: Zephyr.ZephyrGpioDriver base id 0x1002F000 - - instance gpioPayloadPowerLS: Zephyr.ZephyrGpioDriver base id 0x10030000 - - instance gpioPayloadBatteryLS: Zephyr.ZephyrGpioDriver base id 0x10031000 - - instance fsSpace: Components.FsSpace base id 0x10032000 + instance fsSpace: Components.FsSpace base id 0x1002E000 - instance face4LoadSwitch: Components.LoadSwitch base id 0x10033000 + instance face4LoadSwitch: Components.LoadSwitch base id 0x1002F000 - instance face0LoadSwitch: Components.LoadSwitch base id 0x10034000 + instance face0LoadSwitch: Components.LoadSwitch base id 0x10030000 - instance face1LoadSwitch: Components.LoadSwitch base id 0x10035000 + instance face1LoadSwitch: Components.LoadSwitch base id 0x10031000 - instance face2LoadSwitch: Components.LoadSwitch base id 0x10036000 + instance face2LoadSwitch: Components.LoadSwitch base id 0x10032000 - instance face3LoadSwitch: Components.LoadSwitch base id 0x10037000 + instance face3LoadSwitch: Components.LoadSwitch base id 0x10033000 - instance face5LoadSwitch: Components.LoadSwitch base id 0x10038000 + instance face5LoadSwitch: Components.LoadSwitch base id 0x10034000 - instance payloadPowerLoadSwitch: Components.LoadSwitch base id 0x10039000 + instance payloadPowerLoadSwitch: Components.LoadSwitch base id 0x10035000 - instance payloadBatteryLoadSwitch: Components.LoadSwitch base id 0x1003A000 + instance payloadBatteryLoadSwitch: Components.LoadSwitch base id 0x10036000 - instance resetManager: Components.ResetManager base id 0x1003B000 + instance resetManager: Components.ResetManager base id 0x10037000 - instance powerMonitor: Components.PowerMonitor base id 0x1003C000 + instance powerMonitor: Components.PowerMonitor base id 0x10038000 - instance ina219SysManager: Drv.Ina219Manager base id 0x1003D000 + instance ina219SysManager: Drv.Ina219Manager base id 0x10039000 - instance ina219SolManager: Drv.Ina219Manager base id 0x1003E000 + instance ina219SolManager: Drv.Ina219Manager base id 0x1003A000 - instance startupManager: Components.StartupManager base id 0x1003F000 + instance startupManager: Components.StartupManager base id 0x1003B000 } From df73d45da7b4de88532a1a984db71cfbac1efcbe Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Sat, 15 Nov 2025 02:39:32 -0500 Subject: [PATCH 10/34] Lots of debugging statements, cannot identify header --- .../PayloadHandler/PayloadHandler.cpp | 285 +++++++++++------- .../PayloadHandler/PayloadHandler.fpp | 19 +- .../PayloadHandler/PayloadHandler.hpp | 24 +- .../ReferenceDeployment/Top/instances.fpp | 4 +- .../ReferenceDeployment/Top/topology.fpp | 10 +- .../project/config/TlmPacketizerCfg.hpp | 2 +- 6 files changed, 215 insertions(+), 129 deletions(-) diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index 575306d5..f832f257 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -16,17 +16,20 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -PayloadHandler ::PayloadHandler(const char* const compName) +PayloadHandler ::PayloadHandler(const char* const compName) : PayloadHandlerComponentBase(compName), m_protocolBufferSize(0), - m_imageBufferUsed(0) { + m_fileOpen(false) { // Initialize protocol buffer to zero memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); } PayloadHandler ::~PayloadHandler() { - // Clean up any allocated image buffer - deallocateImageBuffer(); + // Close file if still open + if (m_fileOpen) { + m_file.close(); + m_fileOpen = false; + } } @@ -41,7 +44,10 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c // Check if we received data successfully if (status != Drv::ByteStreamStatus::OP_OK) { - // TODO - log error event? + // Log error and abort if receiving + if (m_receiving && m_fileOpen) { + handleFileError(); + } return; } @@ -50,31 +56,48 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c return; } - // Get the data from the incoming buffer + // Get the data from the UART driver's buffer (we don't own it, just read it) const U8* data = buffer.getData(); - const U32 dataSize = static_cast(buffer.getSize()); - - // Unclear if this works as intended if data flow is interrupted + U32 dataSize = static_cast(buffer.getSize()); + + // DEBUG: Log first 8 bytes of EVERY incoming chunk + if (dataSize >= 8) { + this->log_ACTIVITY_LO_RawDataDump( + data[0], data[1], data[2], data[3], + data[4], data[5], data[6], data[7] + ); + } - if (m_receiving && m_imageBuffer.isValid()) { - // Currently receiving image data - accumulate into large buffer + if (m_receiving && m_fileOpen) { + // Currently receiving image data - write directly to file - // First accumulate the new data - if (!accumulateImageData(data, dataSize)) { - // Image buffer overflow - this->log_WARNING_HI_ImageDataOverflow(); - deallocateImageBuffer(); - m_receiving = false; + // Calculate how much to write (don't exceed expected size) + U32 remaining = m_expected_size - m_bytes_received; + U32 toWrite = (dataSize < remaining) ? dataSize : remaining; + + // Write chunk to file + if (!writeChunkToFile(data, toWrite)) { + // Write failed + handleFileError(); return; } + m_bytes_received += toWrite; + // Check if we've received all expected data - if (m_expected_size > 0 && m_imageBufferUsed >= m_expected_size) { - // We have all the data! Trim to exact size (in case we got too) - m_imageBufferUsed = m_expected_size; + if (m_bytes_received >= m_expected_size) { + // Image is complete! + finalizeImageTransfer(); - // Image is complete - processCompleteImage(); + // If there's extra data after the image (e.g., or next header), + // push it to protocol buffer + U32 extraBytes = dataSize - toWrite; + if (extraBytes > 0) { + const U8* extraData = data + toWrite; + if (accumulateProtocolData(extraData, extraBytes)) { + processProtocolBuffer(); + } + } } } else { // Not receiving image - accumulate protocol data @@ -84,9 +107,18 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c accumulateProtocolData(data, dataSize); } + // Log what we have in protocol buffer for debugging + if (m_protocolBufferSize > 0) { + this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); + } + // Process protocol buffer to detect image headers/commands processProtocolBuffer(); } + + // 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); } // ---------------------------------------------------------------------- @@ -141,6 +173,46 @@ bool PayloadHandler ::accumulateProtocolData(const U8* data, U32 size) { void PayloadHandler ::processProtocolBuffer() { // Protocol: [4-byte little-endian uint32][image data] + // Log parse attempt for debugging + if (m_protocolBufferSize > 0) { + this->log_ACTIVITY_LO_HeaderParseAttempt(m_protocolBufferSize); + + // Dump first 8 bytes for debugging + if (m_protocolBufferSize >= 8) { + this->log_ACTIVITY_LO_RawDataDump( + m_protocolBuffer[0], m_protocolBuffer[1], m_protocolBuffer[2], m_protocolBuffer[3], + m_protocolBuffer[4], m_protocolBuffer[5], m_protocolBuffer[6], m_protocolBuffer[7] + ); + } + } + + // Search for anywhere in the buffer (not just at position 0) + I32 headerStart = -1; + 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) { + // No header found - if buffer is nearly full, discard old data + if (m_protocolBufferSize > PROTOCOL_BUFFER_SIZE - 64) { + // Keep last 32 bytes in case header is split + memmove(m_protocolBuffer, &m_protocolBuffer[m_protocolBufferSize - 32], 32); + m_protocolBufferSize = 32; + } + 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 if (m_protocolBufferSize >= HEADER_SIZE) { // Check for at the beginning if (!isImageStartCommand(m_protocolBuffer, m_protocolBufferSize)) { @@ -184,40 +256,56 @@ void PayloadHandler ::processProtocolBuffer() { return; // Invalid header } - // Valid header! Allocate buffer - if (allocateImageBuffer()) { - m_receiving = true; - m_bytes_received = 0; - m_expected_size = imageSize; - - // Generate filename - char filename[64]; - snprintf(filename, sizeof(filename), "/mnt/data/img_%03d.jpg", m_data_file_count++); - m_currentFilename = filename; - - this->log_ACTIVITY_LO_ImageHeaderReceived(); - - // Remove header - U32 remainingSize = m_protocolBufferSize - HEADER_SIZE; - if (remainingSize > 0) { - memmove(m_protocolBuffer, - &m_protocolBuffer[HEADER_SIZE], - remainingSize); - } - m_protocolBufferSize = remainingSize; + // Valid header! Open file immediately for streaming + m_receiving = true; + m_bytes_received = 0; + m_expected_size = imageSize; + + // Generate filename + char filename[64]; + snprintf(filename, sizeof(filename), "/mnt/data/img_%03d.jpg", m_data_file_count++); + 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; + this->log_ACTIVITY_LO_ImageHeaderReceived(); + + // 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 + if (m_protocolBufferSize > 0) { + U32 toWrite = (m_protocolBufferSize < m_expected_size) ? m_protocolBufferSize : m_expected_size; - // Transfer any remaining data (image data) to image buffer - if (m_protocolBufferSize > 0) { - if (!accumulateImageData(m_protocolBuffer, m_protocolBufferSize)) { - this->log_WARNING_HI_ImageDataOverflow(); - deallocateImageBuffer(); - m_receiving = false; + if (writeChunkToFile(m_protocolBuffer, toWrite)) { + m_bytes_received += toWrite; + + // Check if complete already + if (m_bytes_received >= m_expected_size) { + finalizeImageTransfer(); } - clearProtocolBuffer(); + } else { + handleFileError(); } - } else { - // Buffer allocation failed - this->log_WARNING_HI_BufferAllocationFailed(IMAGE_BUFFER_SIZE); + + clearProtocolBuffer(); } } } @@ -227,72 +315,65 @@ void PayloadHandler ::clearProtocolBuffer() { memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); } -bool PayloadHandler ::allocateImageBuffer() { - // Request buffer from BufferManager - m_imageBuffer = this->allocate_out(0, IMAGE_BUFFER_SIZE); - - // Check if allocation succeeded - if (!m_imageBuffer.isValid() || m_imageBuffer.getSize() < IMAGE_BUFFER_SIZE) { - this->log_WARNING_HI_BufferAllocationFailed(IMAGE_BUFFER_SIZE); - deallocateImageBuffer(); +bool PayloadHandler ::writeChunkToFile(const U8* data, U32 size) { + if (!m_fileOpen || size == 0) { return false; } - m_imageBufferUsed = 0; + // 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; + } + return true; } -void PayloadHandler ::deallocateImageBuffer() { - if (m_imageBuffer.isValid()) { - this->deallocate_out(0, m_imageBuffer); - m_imageBuffer = Fw::Buffer(); // Reset to invalid buffer +void PayloadHandler ::finalizeImageTransfer() { + if (!m_fileOpen) { + return; } - m_imageBufferUsed = 0; -} -bool PayloadHandler ::accumulateImageData(const U8* data, U32 size) { - FW_ASSERT(m_imageBuffer.isValid()); + // Close the file + m_file.close(); + m_fileOpen = false; - // Check if we have space - if (m_imageBufferUsed + size > m_imageBuffer.getSize()) { - return false; - } + // Log success + Fw::LogStringArg pathArg(m_currentFilename.c_str()); + this->log_ACTIVITY_HI_DataReceived(m_bytes_received, pathArg); - // Copy data into image buffer - memcpy(&m_imageBuffer.getData()[m_imageBufferUsed], data, size); - m_imageBufferUsed += size; - m_bytes_received += size; - - return true; + // Reset state + m_receiving = false; + m_bytes_received = 0; + m_expected_size = 0; } -void PayloadHandler ::processCompleteImage() { - FW_ASSERT(m_imageBuffer.isValid()); - - // Write image to file - Os::File::Status status = m_file.open(m_currentFilename.c_str(), Os::File::OPEN_WRITE); - - if (status == Os::File::OP_OK) { - // Os::File::write expects FwSizeType& for size parameter - FwSizeType sizeToWrite = static_cast(m_imageBufferUsed); - status = m_file.write(m_imageBuffer.getData(), sizeToWrite, Os::File::WaitType::NO_WAIT); +void PayloadHandler ::handleFileError() { + // Close file if open + if (m_fileOpen) { m_file.close(); - - if (status == Os::File::OP_OK) { - // Success! sizeToWrite now contains actual bytes written - Fw::LogStringArg pathArg(m_currentFilename.c_str()); - this->log_ACTIVITY_HI_DataReceived(m_imageBufferUsed, pathArg); - } else { - // TODO - log write error - } - } else { - // TODO - log open error + m_fileOpen = false; } - // Clean up - deallocateImageBuffer(); + // 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; + clearProtocolBuffer(); } I32 PayloadHandler ::findImageEndMarker(const U8* data, U32 size) { diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp index 1d28fa19..30f5a0e7 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp @@ -4,7 +4,7 @@ module Components { # One async command/port is required for active components # This should be overridden by the developers with a useful command/port - @ TODO + @ Type in "snap" to capture an image sync command SEND_COMMAND(cmd: string) # Command to send data over UART event CommandError(cmd: string) severity warning high format "Failed to send {} command over UART" @@ -23,15 +23,24 @@ module Components { event ImageDataOverflow() severity warning high format "Image data overflow - buffer full" + event ProtocolBufferDebug(bufSize: U32, firstByte: U8) severity activity low format "Protocol buffer: {} bytes, first: 0x{x}" + + event HeaderParseAttempt(bufSize: U32) severity activity low format "Attempting header parse with {} bytes" + + event RawDataDump(byte0: U8, byte1: U8, byte2: U8, byte3: U8, byte4: U8, byte5: U8, byte6: U8, byte7: U8) severity activity low format "Raw: [{x} {x} {x} {x} {x} {x} {x} {x}]" + output port out_port: Drv.ByteStreamSend sync input port in_port: Drv.ByteStreamData - @ Port for allocating buffers for image data - output port allocate: Fw.BufferGet + @ Return RX buffers to UART driver (driver will deallocate to BufferManager) + output port bufferReturn: Fw.BufferSend + + #@ Port for allocating buffers for image data + #output port allocate: Fw.BufferGet - @ Port for deallocating buffers - output port deallocate: Fw.BufferSend + #@ Port for deallocating buffers + #output port deallocate: Fw.BufferSend ############################################################################## #### Uncomment the following examples to start customizing your component #### diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp index caba8e1d..af2f6bb5 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp @@ -35,16 +35,12 @@ class PayloadHandler final : public PayloadHandlerComponentBase { 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 = 2048; + static constexpr U32 PROTOCOL_BUFFER_SIZE = 128; // Just enough for header U8 m_protocolBuffer[PROTOCOL_BUFFER_SIZE]; U32 m_protocolBufferSize = 0; - - // Large image buffer (dynamic allocation via BufferManager) - Fw::Buffer m_imageBuffer; - U32 m_imageBufferUsed = 0; // Bytes used in image buffer - static constexpr U32 IMAGE_BUFFER_SIZE = 256 * 1024; // 256 KB for images // Protocol constants for image transfer // Protocol: [4-byte uint32][image data] @@ -99,19 +95,15 @@ class PayloadHandler final : public PayloadHandlerComponentBase { //! Clear the protocol buffer void clearProtocolBuffer(); - //! Allocate image buffer from BufferManager + //! Write data chunk directly to open file //! Returns true on success - bool allocateImageBuffer(); - - //! Deallocate image buffer - void deallocateImageBuffer(); + bool writeChunkToFile(const U8* data, U32 size); - //! Accumulate image data into dynamically allocated buffer - //! Returns true on success, false on overflow - bool accumulateImageData(const U8* data, U32 size); + //! Close file and finalize image transfer + void finalizeImageTransfer(); - //! Process complete image (write to file, send event, etc.) - void processCompleteImage(); + //! Handle file write error + void handleFileError(); //! Check if buffer contains image end marker //! Returns position of marker start, or -1 if not found diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index de525d4c..8532bf49 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -120,8 +120,8 @@ module ReferenceDeployment { """ phase Fpp.ToCpp.Phases.configComponents """ memset(&ConfigObjects::ReferenceDeployment_payloadBufferManager::bins, 0, sizeof(ConfigObjects::ReferenceDeployment_payloadBufferManager::bins)); - // Configure for 256 KB image buffers, 2 buffers - ConfigObjects::ReferenceDeployment_payloadBufferManager::bins.bins[0].bufferSize = 256 * 1024; + // 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 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 24000738..fc86b3ea 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -216,10 +216,14 @@ module ReferenceDeployment { payload.out_port -> peripheralUartDriver.$send peripheralUartDriver.$recv -> payload.in_port - # Buffer management for image data - payload.allocate -> payloadBufferManager.bufferGetCallee - payload.deallocate -> payloadBufferManager.bufferSendIn + # Buffer return path (critical! - matches ComStub pattern) + payload.bufferReturn -> peripheralUartDriver.recvReturnIn + + # UART driver allocates/deallocates from BufferManager + peripheralUartDriver.allocate -> payloadBufferManager.bufferGetCallee + peripheralUartDriver.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/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 68e52a32..4a26c8c9 100644 --- a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp +++ b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp @@ -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 From 0beaf5b7605892a42db95cfa48a6314c8bf89ec9 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Sat, 15 Nov 2025 19:57:30 -0500 Subject: [PATCH 11/34] More debugging, fixed fpp constants.fpp --- .../PayloadHandler/PayloadHandler.cpp | 82 +++++++++++++++---- .../PayloadHandler/PayloadHandler.fpp | 4 + .../project/config/FpConstants.fpp | 2 +- 3 files changed, 72 insertions(+), 16 deletions(-) diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index f832f257..4dec3c6b 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -48,11 +48,16 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c if (m_receiving && m_fileOpen) { handleFileError(); } + // CRITICAL: Must return buffer even on error to prevent leak + if (buffer.isValid()) { + this->bufferReturn_out(0, buffer); + } return; } // Check if buffer is valid if (!buffer.isValid()) { + // Buffer is invalid but status was OK - unusual case, just return return; } @@ -60,8 +65,8 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c const U8* data = buffer.getData(); U32 dataSize = static_cast(buffer.getSize()); - // DEBUG: Log first 8 bytes of EVERY incoming chunk - if (dataSize >= 8) { + // DEBUG: Log first 8 bytes ONLY if not receiving (reduces load during transfer) + if (!m_receiving && dataSize >= 8) { this->log_ACTIVITY_LO_RawDataDump( data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7] @@ -78,12 +83,20 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c // Write chunk to file if (!writeChunkToFile(data, toWrite)) { // Write failed + this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write failed")); handleFileError(); + // CRITICAL: Return buffer even on error + this->bufferReturn_out(0, buffer); return; } m_bytes_received += toWrite; + // Log progress less frequently to reduce system load (every 512 bytes) + if ((m_bytes_received % 512) < 64) { + this->log_ACTIVITY_LO_ImageTransferProgress(m_bytes_received, m_expected_size); + } + // Check if we've received all expected data if (m_bytes_received >= m_expected_size) { // Image is complete! @@ -101,10 +114,27 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c } } 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 and retry + // Protocol buffer overflow - clear old data and keep new clearProtocolBuffer(); - accumulateProtocolData(data, dataSize); + // 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; + } } // Log what we have in protocol buffer for debugging @@ -188,19 +218,29 @@ void PayloadHandler ::processProtocolBuffer() { // Search for anywhere in the buffer (not just at position 0) I32 headerStart = -1; - for (U32 i = 0; i <= m_protocolBufferSize - IMG_START_LEN; ++i) { - if (isImageStartCommand(&m_protocolBuffer[i], m_protocolBufferSize - i)) { - headerStart = static_cast(i); - break; + + // 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) { // No header found - if buffer is nearly full, discard old data - if (m_protocolBufferSize > PROTOCOL_BUFFER_SIZE - 64) { - // Keep last 32 bytes in case header is split - memmove(m_protocolBuffer, &m_protocolBuffer[m_protocolBufferSize - 32], 32); - m_protocolBufferSize = 32; + // 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; } @@ -210,9 +250,18 @@ void PayloadHandler ::processProtocolBuffer() { U32 remaining = m_protocolBufferSize - static_cast(headerStart); memmove(m_protocolBuffer, &m_protocolBuffer[headerStart], remaining); m_protocolBufferSize = remaining; + + // Log that we found and moved the header + this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); } - // Now check if we have the complete header + // 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)) { @@ -261,9 +310,12 @@ void PayloadHandler ::processProtocolBuffer() { m_bytes_received = 0; m_expected_size = imageSize; - // Generate filename + // Log the extracted size + this->log_ACTIVITY_HI_ImageSizeExtracted(imageSize); + + // Generate filename - save to root filesystem char filename[64]; - snprintf(filename, sizeof(filename), "/mnt/data/img_%03d.jpg", m_data_file_count++); + snprintf(filename, sizeof(filename), "/img_%03d.jpg", m_data_file_count++); m_currentFilename = filename; // Open file for writing diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp index 30f5a0e7..772e0835 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp @@ -17,6 +17,10 @@ module Components { event ImageHeaderReceived() severity activity low format "Received image header" + event ImageSizeExtracted(imageSize: U32) severity activity high format "Image size from header: {} bytes" + + event ImageTransferProgress(received: U32, expected: U32) severity activity low format "Transfer progress: {}/{} bytes" + event UartReceived() severity activity low format "Received UART data" event BufferAllocationFailed(buffer_size: U32) severity warning high format "Failed to allocate buffer of size {}" 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 From 79e5fae44561397b8d1d6d62eb953b9215fa2c2f Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Mon, 17 Nov 2025 01:56:23 -0500 Subject: [PATCH 12/34] Initial Handshake Protocol Component --- .../PayloadHandler/PayloadHandler.cpp | 23 ++++++++++++++++++- .../PayloadHandler/PayloadHandler.hpp | 3 +++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp index 4dec3c6b..3d7ee009 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp @@ -1,6 +1,6 @@ // ====================================================================== // \title PayloadHandler.cpp -// \author robertpendergrast +// \author robertpendergrast, moisesmata // \brief cpp file for PayloadHandler component implementation class // ====================================================================== #include "Os/File.hpp" @@ -92,6 +92,9 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c m_bytes_received += toWrite; + // Send ACK after successfully writing chunk + sendAck(); + // Log progress less frequently to reduce system load (every 512 bytes) if ((m_bytes_received % 512) < 64) { this->log_ACTIVITY_LO_ImageTransferProgress(m_bytes_received, m_expected_size); @@ -332,6 +335,9 @@ void PayloadHandler ::processProtocolBuffer() { m_fileOpen = true; this->log_ACTIVITY_LO_ImageHeaderReceived(); + + // Send ACK to camera after successfully parsing header and opening file + sendAck(); // Remove header from protocol buffer U32 remainingSize = m_protocolBufferSize - HEADER_SIZE; @@ -343,6 +349,7 @@ void PayloadHandler ::processProtocolBuffer() { 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; @@ -404,6 +411,9 @@ void PayloadHandler ::finalizeImageTransfer() { // Log success Fw::LogStringArg pathArg(m_currentFilename.c_str()); this->log_ACTIVITY_HI_DataReceived(m_bytes_received, pathArg); + + // Send final ACK to camera to confirm complete transfer + sendAck(); // Reset state m_receiving = false; @@ -474,4 +484,15 @@ bool PayloadHandler ::isImageStartCommand(const U8* line, U32 length) { return true; } +void PayloadHandler ::sendAck(){ + // Send an acknowledgment over UART + const char* ackMsg = "\n"; + Fw::Buffer ackBuffer( + reinterpret_cast(const_cast(ackMsg)), + strlen(ackMsg) + ); + this->out_port_out(0, ackBuffer); + +} + } // namespace Components diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp index af2f6bb5..500fcd6d 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp +++ b/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp @@ -112,6 +112,9 @@ class PayloadHandler final : public PayloadHandlerComponentBase { //! Parse line for image start command //! Returns true if line is "" bool isImageStartCommand(const U8* line, U32 length); + + //! Send acknowledgment over UART + void sendAck(); }; } // namespace Components From 57d0cd608b4d51d89d8158daf2e6229abf81f3cd Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Mon, 17 Nov 2025 02:51:05 -0500 Subject: [PATCH 13/34] Initial split of PayloadCom and CameraHandler --- .../Components/CMakeLists.txt | 3 +- .../CMakeLists.txt | 10 +-- .../CameraHandler/CameraHandler.cpp | 28 ++++++++ .../CameraHandler/CameraHandler.fpp | 60 +++++++++++++++++ .../CameraHandler/CameraHandler.hpp | 42 ++++++++++++ .../Components/CameraHandler/docs/sdd.md | 66 +++++++++++++++++++ .../Components/PayloadCom/CMakeLists.txt | 36 ++++++++++ .../PayloadCom.cpp} | 34 +++++----- .../PayloadCom.fpp} | 2 +- .../PayloadCom.hpp} | 22 +++---- .../docs/BUFFER_USAGE.md | 10 +-- .../docs/USAGE.md | 10 +-- .../docs/sdd.md | 0 .../ReferenceDeployment/Top/instances.fpp | 2 +- .../ReferenceDeployment/Top/topology.fpp | 2 +- 15 files changed, 280 insertions(+), 47 deletions(-) rename FprimeZephyrReference/Components/{PayloadHandler => CameraHandler}/CMakeLists.txt (72%) create mode 100644 FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp create mode 100644 FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp create mode 100644 FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp create mode 100644 FprimeZephyrReference/Components/CameraHandler/docs/sdd.md create mode 100644 FprimeZephyrReference/Components/PayloadCom/CMakeLists.txt rename FprimeZephyrReference/Components/{PayloadHandler/PayloadHandler.cpp => PayloadCom/PayloadCom.cpp} (93%) rename FprimeZephyrReference/Components/{PayloadHandler/PayloadHandler.fpp => PayloadCom/PayloadCom.fpp} (99%) rename FprimeZephyrReference/Components/{PayloadHandler/PayloadHandler.hpp => PayloadCom/PayloadCom.hpp} (88%) rename FprimeZephyrReference/Components/{PayloadHandler => PayloadCom}/docs/BUFFER_USAGE.md (96%) rename FprimeZephyrReference/Components/{PayloadHandler => PayloadCom}/docs/USAGE.md (96%) rename FprimeZephyrReference/Components/{PayloadHandler => PayloadCom}/docs/sdd.md (100%) diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index d88d956c..7c290c7d 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -14,4 +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}/PayloadHandler/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/PayloadCom/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/CameraHandler/") diff --git a/FprimeZephyrReference/Components/PayloadHandler/CMakeLists.txt b/FprimeZephyrReference/Components/CameraHandler/CMakeLists.txt similarity index 72% rename from FprimeZephyrReference/Components/PayloadHandler/CMakeLists.txt rename to FprimeZephyrReference/Components/CameraHandler/CMakeLists.txt index 15cabfdd..b520fa05 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CameraHandler/CMakeLists.txt @@ -16,9 +16,9 @@ register_fprime_library( AUTOCODER_INPUTS - "${CMAKE_CURRENT_LIST_DIR}/PayloadHandler.fpp" + "${CMAKE_CURRENT_LIST_DIR}/CameraHandler.fpp" SOURCES - "${CMAKE_CURRENT_LIST_DIR}/PayloadHandler.cpp" + "${CMAKE_CURRENT_LIST_DIR}/CameraHandler.cpp" # DEPENDS # MyPackage_MyOtherModule ) @@ -26,10 +26,10 @@ register_fprime_library( ### Unit Tests ### # register_fprime_ut( # AUTOCODER_INPUTS -# "${CMAKE_CURRENT_LIST_DIR}/PayloadHandler.fpp" +# "${CMAKE_CURRENT_LIST_DIR}/CameraHandler.fpp" # SOURCES -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/PayloadHandlerTestMain.cpp" -# "${CMAKE_CURRENT_LIST_DIR}/test/ut/PayloadHandlerTester.cpp" +# "${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..c76b689a --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -0,0 +1,28 @@ +// ====================================================================== +// \title CameraHandler.cpp +// \author moises +// \brief cpp file for CameraHandler component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp" + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +CameraHandler ::CameraHandler(const char* const compName) : CameraHandlerComponentBase(compName) {} + +CameraHandler ::~CameraHandler() {} + +// ---------------------------------------------------------------------- +// Handler implementations for commands +// ---------------------------------------------------------------------- + +void CameraHandler ::TODO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + // TODO + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp new file mode 100644 index 00000000..92231d55 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -0,0 +1,60 @@ +module Components { + @ passive component that handles camera specific payload capabilities + active component CameraHandler { + + # One async command/port is required for active components + # This should be overridden by the developers with a useful command/port + @ TODO + async command TODO opcode 0 + + ############################################################################## + #### 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 + + } +} \ No newline at end of file diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp new file mode 100644 index 00000000..6ec99bdc --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -0,0 +1,42 @@ +// ====================================================================== +// \title CameraHandler.hpp +// \author moises +// \brief hpp file for CameraHandler component implementation class +// ====================================================================== + +#ifndef Components_CameraHandler_HPP +#define Components_CameraHandler_HPP + +#include "FprimeZephyrReference/Components/CameraHandler/CameraHandlerComponentAc.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(); + + private: + // ---------------------------------------------------------------------- + // Handler implementations for commands + // ---------------------------------------------------------------------- + + //! Handler implementation for command TODO + //! + //! TODO + void TODO_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; +}; + +} // 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..8eb34917 --- /dev/null +++ b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md @@ -0,0 +1,66 @@ +# Components::CameraHandler + +passive component that handles camera specific payload capabilities + +## 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/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/PayloadHandler/PayloadHandler.cpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp similarity index 93% rename from FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp rename to FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp index 3d7ee009..0893db06 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.cpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp @@ -1,12 +1,12 @@ // ====================================================================== -// \title PayloadHandler.cpp +// \title PayloadCom.cpp // \author robertpendergrast, moisesmata -// \brief cpp file for PayloadHandler component implementation class +// \brief cpp file for PayloadCom component implementation class // ====================================================================== #include "Os/File.hpp" #include "Fw/Types/Assert.hpp" #include "Fw/Types/BasicTypes.hpp" -#include "FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp" +#include "FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp" #include #include @@ -16,15 +16,15 @@ namespace Components { // Component construction and destruction // ---------------------------------------------------------------------- -PayloadHandler ::PayloadHandler(const char* const compName) - : PayloadHandlerComponentBase(compName), +PayloadCom ::PayloadCom(const char* const compName) + : PayloadComComponentBase(compName), m_protocolBufferSize(0), m_fileOpen(false) { // Initialize protocol buffer to zero memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); } -PayloadHandler ::~PayloadHandler() { +PayloadCom ::~PayloadCom() { // Close file if still open if (m_fileOpen) { m_file.close(); @@ -38,7 +38,7 @@ PayloadHandler ::~PayloadHandler() { // ---------------------------------------------------------------------- -void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { +void PayloadCom ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { this->log_ACTIVITY_LO_UartReceived(); @@ -158,7 +158,7 @@ void PayloadHandler ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, c // Handler implementations for commands // ---------------------------------------------------------------------- -void PayloadHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) { +void PayloadCom ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) { // Append newline to command to send over UART Fw::CmdStringArg tempCmd = cmd; @@ -190,7 +190,7 @@ void PayloadHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, c // Helper method implementations // ---------------------------------------------------------------------- -bool PayloadHandler ::accumulateProtocolData(const U8* data, U32 size) { +bool PayloadCom ::accumulateProtocolData(const U8* data, U32 size) { // Check if we have space for the new data if (m_protocolBufferSize + size > PROTOCOL_BUFFER_SIZE) { return false; @@ -203,7 +203,7 @@ bool PayloadHandler ::accumulateProtocolData(const U8* data, U32 size) { return true; } -void PayloadHandler ::processProtocolBuffer() { +void PayloadCom ::processProtocolBuffer() { // Protocol: [4-byte little-endian uint32][image data] // Log parse attempt for debugging @@ -369,12 +369,12 @@ void PayloadHandler ::processProtocolBuffer() { } } -void PayloadHandler ::clearProtocolBuffer() { +void PayloadCom ::clearProtocolBuffer() { m_protocolBufferSize = 0; memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); } -bool PayloadHandler ::writeChunkToFile(const U8* data, U32 size) { +bool PayloadCom ::writeChunkToFile(const U8* data, U32 size) { if (!m_fileOpen || size == 0) { return false; } @@ -399,7 +399,7 @@ bool PayloadHandler ::writeChunkToFile(const U8* data, U32 size) { return true; } -void PayloadHandler ::finalizeImageTransfer() { +void PayloadCom ::finalizeImageTransfer() { if (!m_fileOpen) { return; } @@ -421,7 +421,7 @@ void PayloadHandler ::finalizeImageTransfer() { m_expected_size = 0; } -void PayloadHandler ::handleFileError() { +void PayloadCom ::handleFileError() { // Close file if open if (m_fileOpen) { m_file.close(); @@ -438,7 +438,7 @@ void PayloadHandler ::handleFileError() { clearProtocolBuffer(); } -I32 PayloadHandler ::findImageEndMarker(const U8* data, U32 size) { +I32 PayloadCom ::findImageEndMarker(const U8* data, U32 size) { // Looking for "\n" or "" const char* marker = ""; @@ -468,7 +468,7 @@ I32 PayloadHandler ::findImageEndMarker(const U8* data, U32 size) { return -1; // Not found } -bool PayloadHandler ::isImageStartCommand(const U8* line, U32 length) { +bool PayloadCom ::isImageStartCommand(const U8* line, U32 length) { const char* command = ""; if (length < IMG_START_LEN) { @@ -484,7 +484,7 @@ bool PayloadHandler ::isImageStartCommand(const U8* line, U32 length) { return true; } -void PayloadHandler ::sendAck(){ +void PayloadCom ::sendAck(){ // Send an acknowledgment over UART const char* ackMsg = "\n"; Fw::Buffer ackBuffer( diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp similarity index 99% rename from FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp rename to FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp index 772e0835..1d45d4d7 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.fpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp @@ -1,6 +1,6 @@ module Components { @ Manager for Nicla Vision - passive component PayloadHandler { + passive component PayloadCom { # One async command/port is required for active components # This should be overridden by the developers with a useful command/port diff --git a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp similarity index 88% rename from FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp rename to FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp index 500fcd6d..371557f5 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/PayloadHandler.hpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp @@ -1,31 +1,31 @@ // ====================================================================== -// \title PayloadHandler.hpp -// \author robertpendergrast -// \brief hpp file for PayloadHandler component implementation class +// \title PayloadCom.hpp +// \author robertpendergrast, moisesmata +// \brief hpp file for PayloadCom component implementation class // ====================================================================== -#ifndef FprimeZephyrReference_PayloadHandler_HPP -#define FprimeZephyrReference_PayloadHandler_HPP +#ifndef FprimeZephyrReference_PayloadCom_HPP +#define FprimeZephyrReference_PayloadCom_HPP #include #include #include "Os/File.hpp" -#include "FprimeZephyrReference/Components/PayloadHandler/PayloadHandlerComponentAc.hpp" +#include "FprimeZephyrReference/Components/PayloadCom/PayloadComComponentAc.hpp" namespace Components { -class PayloadHandler final : public PayloadHandlerComponentBase { +class PayloadCom final : public PayloadComComponentBase { public: // ---------------------------------------------------------------------- // Component construction and destruction // ---------------------------------------------------------------------- - //! Construct PayloadHandler object - PayloadHandler(const char* const compName //!< The component name + //! Construct PayloadCom object + PayloadCom(const char* const compName //!< The component name ); - //! Destroy PayloadHandler object - ~PayloadHandler(); + //! Destroy PayloadCom object + ~PayloadCom(); U8 m_data_file_count = 0; bool m_receiving = false; diff --git a/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md b/FprimeZephyrReference/Components/PayloadCom/docs/BUFFER_USAGE.md similarity index 96% rename from FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md rename to FprimeZephyrReference/Components/PayloadCom/docs/BUFFER_USAGE.md index 84c1e959..9a1dfe55 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/docs/BUFFER_USAGE.md +++ b/FprimeZephyrReference/Components/PayloadCom/docs/BUFFER_USAGE.md @@ -1,8 +1,8 @@ -# PayloadHandler Buffer Strategy +# PayloadCom Buffer Strategy ## Overview -The PayloadHandler uses a **two-buffer strategy** to efficiently handle both protocol/header data and large image data: +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 @@ -25,7 +25,7 @@ The PayloadHandler uses a **two-buffer strategy** to efficiently handle both pro ``` UART Driver (64 byte chunks) ↓ - PayloadHandler + PayloadCom ↓ ┌─────┴─────┐ ↓ ↓ @@ -154,12 +154,12 @@ payload.deallocate -> payloadBufferManager.bufferSendIn ### To Change Buffer Sizes -1. **Protocol buffer** (PayloadHandler.hpp): +1. **Protocol buffer** (PayloadCom.hpp): ```cpp static constexpr U32 PROTOCOL_BUFFER_SIZE = 2048; // Change this ``` -2. **Image buffer** (PayloadHandler.hpp): +2. **Image buffer** (PayloadCom.hpp): ```cpp static constexpr U32 IMAGE_BUFFER_SIZE = 256 * 1024; // Change this ``` diff --git a/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md b/FprimeZephyrReference/Components/PayloadCom/docs/USAGE.md similarity index 96% rename from FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md rename to FprimeZephyrReference/Components/PayloadCom/docs/USAGE.md index f971cf75..3bd9bcfb 100644 --- a/FprimeZephyrReference/Components/PayloadHandler/docs/USAGE.md +++ b/FprimeZephyrReference/Components/PayloadCom/docs/USAGE.md @@ -1,8 +1,8 @@ -# PayloadHandler Usage Guide +# PayloadCom Usage Guide ## Overview -The PayloadHandler receives images from the Nicla Vision camera over UART and saves them to the filesystem. +The PayloadCom receives images from the Nicla Vision camera over UART and saves them to the filesystem. ## Camera Commands @@ -41,7 +41,7 @@ This forwards the command over UART to the camera. - Event: `CommandSuccess` - "Command snap sent successfully" 2. **Image Start** - - PayloadHandler receives `...bytes...` + - PayloadCom receives `...bytes...` - Parses image size from 4-byte little-endian value - Allocates 256 KB buffer from BufferManager - Event: `ImageHeaderReceived` - "Received image header" @@ -91,7 +91,7 @@ Counter increments each time a valid `...` header is received. - Camera sent more data than buffer can hold **Recovery:** -- Increase `IMAGE_BUFFER_SIZE` in PayloadHandler.hpp +- Increase `IMAGE_BUFFER_SIZE` in PayloadCom.hpp - Update BufferManager configuration in instances.fpp ### Command Error @@ -283,7 +283,7 @@ bins[0].bufferSize = 512 * 1024; bins[0].numBuffers = 4; ``` -Must also update `PayloadHandler.hpp`: +Must also update `PayloadCom.hpp`: ```cpp static constexpr U32 IMAGE_BUFFER_SIZE = 512 * 1024; ``` diff --git a/FprimeZephyrReference/Components/PayloadHandler/docs/sdd.md b/FprimeZephyrReference/Components/PayloadCom/docs/sdd.md similarity index 100% rename from FprimeZephyrReference/Components/PayloadHandler/docs/sdd.md rename to FprimeZephyrReference/Components/PayloadCom/docs/sdd.md diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 8532bf49..9eca4870 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -109,7 +109,7 @@ module ReferenceDeployment { instance gpioPayloadBatteryLS: Zephyr.ZephyrGpioDriver base id 0x1002A000 - instance payload: Components.PayloadHandler base id 0x1002B000 + instance payload: Components.PayloadCom base id 0x1002B000 instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x1002C000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index fc86b3ea..d1a196d4 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -212,7 +212,7 @@ module ReferenceDeployment { imuManager.temperatureGet -> lsm6dsoManager.temperatureGet } - connections PayloadHandler { + connections PayloadCom { payload.out_port -> peripheralUartDriver.$send peripheralUartDriver.$recv -> payload.in_port From 958baa30df0d871c15ab5f4ca8805896cca013ac Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Mon, 17 Nov 2025 04:11:56 -0500 Subject: [PATCH 14/34] some reworking of payloadcom to camerahandler --- .../CameraHandler/CameraHandler.cpp | 420 +++++++++++++++++- .../CameraHandler/CameraHandler.fpp | 28 +- .../CameraHandler/CameraHandler.hpp | 61 ++- .../Components/PayloadCom/PayloadCom.cpp | 397 +---------------- .../Components/PayloadCom/PayloadCom.fpp | 37 +- .../Components/PayloadCom/PayloadCom.hpp | 30 -- 6 files changed, 514 insertions(+), 459 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index c76b689a..b64baf63 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -4,6 +4,11 @@ // \brief cpp file for CameraHandler component implementation class // ====================================================================== +#include "Os/File.hpp" +#include "Fw/Types/Assert.hpp" +#include "Fw/Types/BasicTypes.hpp" +#include +#include #include "FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp" namespace Components { @@ -16,13 +21,426 @@ CameraHandler ::CameraHandler(const char* const compName) : CameraHandlerCompone CameraHandler ::~CameraHandler() {} +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { + + // Get the data from the UART driver's buffer (we don't own it, just read it) + const U8* data = buffer.getData(); + U32 dataSize = static_cast(buffer.getSize()); + + 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; + + // Write chunk to file + if (!writeChunkToFile(data, toWrite)) { + // Write failed + this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write failed")); + handleFileError(); + // CRITICAL: Return buffer even on error + this->bufferReturn_out(0, buffer); + return; + } + + m_bytes_received += toWrite; + + // Log progress less frequently to reduce system load (every 512 bytes) + if ((m_bytes_received % 512) < 64) { + this->log_ACTIVITY_LO_ImageTransferProgress(m_bytes_received, m_expected_size); + } + + // 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 + U32 extraBytes = dataSize - toWrite; + if (extraBytes > 0) { + const U8* extraData = data + toWrite; + if (accumulateProtocolData(extraData, extraBytes)) { + processProtocolBuffer(); + } + } + } + } 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; + } + } + + // Log what we have in protocol buffer for debugging + if (m_protocolBufferSize > 0) { + this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); + } + + // Process protocol buffer to detect image headers/commands + processProtocolBuffer(); + } + + // 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); +} + // ---------------------------------------------------------------------- // Handler implementations for commands // ---------------------------------------------------------------------- -void CameraHandler ::TODO_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { +void CameraHandler ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // TODO this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +void CameraHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) { + // Append newline to command to send over UART + Fw::CmdStringArg tempCmd = cmd; + tempCmd += "\n"; + Fw::Buffer commandBuffer( + reinterpret_cast(const_cast(tempCmd.toChar())), + tempCmd.length() + ); + + // Send command over output port + Drv::ByteStreamStatus sendStatus = this->out_port_out(0, commandBuffer); + + Fw::LogStringArg logCmd(cmd); + + // Log success or failure + if (sendStatus != Drv::ByteStreamStatus::OP_OK) { + this->log_WARNING_HI_CommandError(logCmd); + this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); + return; + } + else { + 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] + + // Log parse attempt for debugging + if (m_protocolBufferSize > 0) { + this->log_ACTIVITY_LO_HeaderParseAttempt(m_protocolBufferSize); + + // Dump first 8 bytes for debugging + if (m_protocolBufferSize >= 8) { + this->log_ACTIVITY_LO_RawDataDump( + m_protocolBuffer[0], m_protocolBuffer[1], m_protocolBuffer[2], m_protocolBuffer[3], + m_protocolBuffer[4], m_protocolBuffer[5], m_protocolBuffer[6], m_protocolBuffer[7] + ); + } + } + + // 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) { + // 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; + + // Log that we found and moved the header + this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); + } + + // 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; + + // Log the extracted size + this->log_ACTIVITY_HI_ImageSizeExtracted(imageSize); + + // Generate filename - save to root filesystem + char filename[64]; + snprintf(filename, sizeof(filename), "/img_%03d.jpg", m_data_file_count++); + 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; + this->log_ACTIVITY_LO_ImageHeaderReceived(); + + // Send ACK to camera after successfully parsing header and opening file + sendAck(); + + // 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; + } + + return true; +} + +void CameraHandler ::finalizeImageTransfer() { + if (!m_fileOpen) { + return; + } + + // Close the file + m_file.close(); + m_fileOpen = false; + + // Log success + Fw::LogStringArg pathArg(m_currentFilename.c_str()); + this->log_ACTIVITY_HI_DataReceived(m_bytes_received, pathArg); + + // Send final ACK to camera to confirm complete transfer + sendAck(); + + // Reset state + m_receiving = false; + m_bytes_received = 0; + m_expected_size = 0; +} + +void CameraHandler ::handleFileError() { + // Close file if open + if (m_fileOpen) { + m_file.close(); + m_fileOpen = false; + } + + // 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; + clearProtocolBuffer(); +} + +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; +} } // namespace Components diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 92231d55..3d3572c5 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -1,11 +1,33 @@ module Components { - @ passive component that handles camera specific payload capabilities + @ active component that handles camera specific payload capabilities active component CameraHandler { # One async command/port is required for active components # This should be overridden by the developers with a useful command/port - @ TODO - async command TODO opcode 0 + @ Type in "snap" to capture an image + async command TAKE_IMAGE() # Command to send data over UART + + async command SEND_COMMAND(cmd: string) # Command to send data over UART + + #async command SET_ATTRIBUTES + + event ImageHeaderReceived() severity activity low format "Received image header" + + event ImageSizeExtracted(imageSize: U32) severity activity high format "Image size from header: {} bytes" + + event ImageTransferProgress(received: U32, expected: U32) severity activity low format "Transfer progress: {}/{} bytes" + + event ImageDataOverflow() severity warning high format "Image data overflow - buffer full" + + event ProtocolBufferDebug(bufSize: U32, firstByte: U8) severity activity low format "Protocol buffer: {} bytes, first: 0x{x}" + + event HeaderParseAttempt(bufSize: U32) severity activity low format "Attempting header parse with {} bytes" + + @ Sends command to PayloadCom to be forwarded over UART + output port commandOut: Drv.ByteStreamData + + @ Receives data from PayloadCom over UART, handles image file saving logic + sync input port dataIn: Drv.ByteStreamData ############################################################################## #### Uncomment the following examples to start customizing your component #### diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index 6ec99bdc..c4117d64 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -7,7 +7,10 @@ #ifndef Components_CameraHandler_HPP #define Components_CameraHandler_HPP +#include +#include #include "FprimeZephyrReference/Components/CameraHandler/CameraHandlerComponentAc.hpp" +#include "Os/File.hpp"> namespace Components { @@ -24,17 +27,65 @@ class CameraHandler final : public CameraHandlerComponentBase { //! Destroy CameraHandler object ~CameraHandler(); + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for dataIn + //! + //! Receives data from PayloadCom over UART, handles image file saving logic + 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 TODO + //! Handler implementation for command TAKE_IMAGE //! - //! TODO - void TODO_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq //!< The command sequence number - ) override; + //! 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; + + U8 m_data_file_count = 0; + bool m_receiving = false; + U32 m_bytes_received = 0; + + 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("") + + // 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 }; } // namespace Components diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp index 0893db06..bb7ff0c1 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp @@ -44,111 +44,17 @@ void PayloadCom ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const // Check if we received data successfully if (status != Drv::ByteStreamStatus::OP_OK) { - // Log error and abort if receiving - if (m_receiving && m_fileOpen) { - handleFileError(); - } - // CRITICAL: Must return buffer even on error to prevent leak + // Must return buffer even on error to prevent leak if (buffer.isValid()) { this->bufferReturn_out(0, buffer); } return; } - // Check if buffer is valid - if (!buffer.isValid()) { - // Buffer is invalid but status was OK - unusual case, just return - return; - } - - // Get the data from the UART driver's buffer (we don't own it, just read it) - const U8* data = buffer.getData(); - U32 dataSize = static_cast(buffer.getSize()); + this->uartDataOut_out(0, buffer, Drv::ByteStreamStatus::OP_OK); - // DEBUG: Log first 8 bytes ONLY if not receiving (reduces load during transfer) - if (!m_receiving && dataSize >= 8) { - this->log_ACTIVITY_LO_RawDataDump( - data[0], data[1], data[2], data[3], - data[4], data[5], data[6], data[7] - ); - } - - 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; - - // Write chunk to file - if (!writeChunkToFile(data, toWrite)) { - // Write failed - this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write failed")); - handleFileError(); - // CRITICAL: Return buffer even on error - this->bufferReturn_out(0, buffer); - return; - } - - m_bytes_received += toWrite; - - // Send ACK after successfully writing chunk - sendAck(); - - // Log progress less frequently to reduce system load (every 512 bytes) - if ((m_bytes_received % 512) < 64) { - this->log_ACTIVITY_LO_ImageTransferProgress(m_bytes_received, m_expected_size); - } - - // 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 - U32 extraBytes = dataSize - toWrite; - if (extraBytes > 0) { - const U8* extraData = data + toWrite; - if (accumulateProtocolData(extraData, extraBytes)) { - processProtocolBuffer(); - } - } - } - } 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; - } - } + sendAck(); - 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; - } - } - - // Log what we have in protocol buffer for debugging - if (m_protocolBufferSize > 0) { - this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); - } - - // Process protocol buffer to detect image headers/commands - processProtocolBuffer(); - } - // 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); @@ -186,303 +92,6 @@ void PayloadCom ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } -// ---------------------------------------------------------------------- -// Helper method implementations -// ---------------------------------------------------------------------- - -bool PayloadCom ::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 PayloadCom ::processProtocolBuffer() { - // Protocol: [4-byte little-endian uint32][image data] - - // Log parse attempt for debugging - if (m_protocolBufferSize > 0) { - this->log_ACTIVITY_LO_HeaderParseAttempt(m_protocolBufferSize); - - // Dump first 8 bytes for debugging - if (m_protocolBufferSize >= 8) { - this->log_ACTIVITY_LO_RawDataDump( - m_protocolBuffer[0], m_protocolBuffer[1], m_protocolBuffer[2], m_protocolBuffer[3], - m_protocolBuffer[4], m_protocolBuffer[5], m_protocolBuffer[6], m_protocolBuffer[7] - ); - } - } - - // 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) { - // 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; - - // Log that we found and moved the header - this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); - } - - // 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; - - // Log the extracted size - this->log_ACTIVITY_HI_ImageSizeExtracted(imageSize); - - // Generate filename - save to root filesystem - char filename[64]; - snprintf(filename, sizeof(filename), "/img_%03d.jpg", m_data_file_count++); - 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; - this->log_ACTIVITY_LO_ImageHeaderReceived(); - - // Send ACK to camera after successfully parsing header and opening file - sendAck(); - - // 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 PayloadCom ::clearProtocolBuffer() { - m_protocolBufferSize = 0; - memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); -} - -bool PayloadCom ::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; - } - - return true; -} - -void PayloadCom ::finalizeImageTransfer() { - if (!m_fileOpen) { - return; - } - - // Close the file - m_file.close(); - m_fileOpen = false; - - // Log success - Fw::LogStringArg pathArg(m_currentFilename.c_str()); - this->log_ACTIVITY_HI_DataReceived(m_bytes_received, pathArg); - - // Send final ACK to camera to confirm complete transfer - sendAck(); - - // Reset state - m_receiving = false; - m_bytes_received = 0; - m_expected_size = 0; -} - -void PayloadCom ::handleFileError() { - // Close file if open - if (m_fileOpen) { - m_file.close(); - m_fileOpen = false; - } - - // 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; - clearProtocolBuffer(); -} - -I32 PayloadCom ::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 PayloadCom ::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; -} void PayloadCom ::sendAck(){ // Send an acknowledgment over UART diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp index 1d45d4d7..5f4bf093 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp @@ -2,49 +2,34 @@ module Components { @ Manager for Nicla Vision passive component PayloadCom { - # One async command/port is required for active components - # This should be overridden by the developers with a useful command/port - @ Type in "snap" to capture an image - sync command SEND_COMMAND(cmd: string) # Command to send data over UART + event CommandForwardError(cmd: string) severity warning high format "Failed to send {} command over UART" - event CommandError(cmd: string) severity warning high format "Failed to send {} command over UART" - - event CommandSuccess(cmd: string) severity activity high format "Command {} sent successfully" + event CommandForwardSuccess(cmd: string) severity activity high format "Command {} sent successfully" event DataReceived( data: U8, path: string) severity activity high format "Stored {} bytes of payload data to {}" event ByteReceived( byte: U8) severity activity low format "Received byte: {}" - event ImageHeaderReceived() severity activity low format "Received image header" - - event ImageSizeExtracted(imageSize: U32) severity activity high format "Image size from header: {} bytes" - - event ImageTransferProgress(received: U32, expected: U32) severity activity low format "Transfer progress: {}/{} bytes" - event UartReceived() severity activity low format "Received UART data" event BufferAllocationFailed(buffer_size: U32) severity warning high format "Failed to allocate buffer of size {}" - event ImageDataOverflow() severity warning high format "Image data overflow - buffer full" - - event ProtocolBufferDebug(bufSize: U32, firstByte: U8) severity activity low format "Protocol buffer: {} bytes, first: 0x{x}" - - event HeaderParseAttempt(bufSize: U32) severity activity low format "Attempting header parse with {} bytes" - event RawDataDump(byte0: U8, byte1: U8, byte2: U8, byte3: U8, byte4: U8, byte5: U8, byte6: U8, byte7: U8) severity activity low format "Raw: [{x} {x} {x} {x} {x} {x} {x} {x}]" - output port out_port: Drv.ByteStreamSend + @ Receives the desired command to forward through the payload UART + sync input port commandIn: Drv.ByteStreamData - sync input port in_port: Drv.ByteStreamData + @ Receives data from the UART, handles handshake protocol logic + sync 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 - #@ Port for allocating buffers for image data - #output port allocate: Fw.BufferGet - - #@ Port for deallocating buffers - #output port deallocate: Fw.BufferSend + @ Sends data that is received by the uartDataIn port to the desired payload handler component + output port uartDataOut: Drv.ByteStreamSend ############################################################################## #### Uncomment the following examples to start customizing your component #### diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp index 371557f5..6aa0aa17 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp @@ -27,36 +27,6 @@ class PayloadCom final : public PayloadComComponentBase { //! Destroy PayloadCom object ~PayloadCom(); - U8 m_data_file_count = 0; - bool m_receiving = false; - U32 m_bytes_received = 0; - - 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("") - - // 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 private: // ---------------------------------------------------------------------- From a6d91a4494da8927227a8b40c644b29d83da6828 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Mon, 17 Nov 2025 20:56:52 -0500 Subject: [PATCH 15/34] Initial kinda working, only one ack is sent --- .../CameraHandler/CameraHandler.cpp | 69 +++++++++++------ .../CameraHandler/CameraHandler.fpp | 28 +++++-- .../CameraHandler/CameraHandler.hpp | 43 ++++++++++- .../Components/PayloadCom/PayloadCom.cpp | 76 +++++++------------ .../Components/PayloadCom/PayloadCom.fpp | 17 ++--- .../Components/PayloadCom/PayloadCom.hpp | 59 +++----------- .../ReferenceDeployment/Top/instances.fpp | 5 ++ .../ReferenceDeployment/Top/topology.fpp | 13 +++- 8 files changed, 163 insertions(+), 147 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index b64baf63..fe2d7250 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -2,6 +2,7 @@ // \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" @@ -19,18 +20,49 @@ namespace Components { CameraHandler ::CameraHandler(const char* const compName) : CameraHandlerComponentBase(compName) {} -CameraHandler ::~CameraHandler() {} +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(); + } + // Return buffer even on error + if (buffer.isValid()) { + this->bufferReturn_out(0, buffer); + } + return; + } - // Get the data from the UART driver's buffer (we don't own it, just read it) + // 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()); + // DEBUG: Log first 8 bytes ONLY if not receiving (reduces load during transfer) + if (!m_receiving && dataSize >= 8) { + this->log_ACTIVITY_LO_RawDataDump( + data[0], data[1], data[2], data[3], + data[4], data[5], data[6], data[7] + ); + } + if (m_receiving && m_fileOpen) { // Currently receiving image data - write directly to file @@ -43,7 +75,7 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con // Write failed this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write failed")); handleFileError(); - // CRITICAL: Return buffer even on error + // Return buffer even on error this->bufferReturn_out(0, buffer); return; } @@ -104,8 +136,7 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con processProtocolBuffer(); } - // CRITICAL: Return buffer to driver so it can deallocate to BufferManager - // This matches the ComStub pattern: driver allocates, handler processes, handler returns + // Return buffer to allow reuse this->bufferReturn_out(0, buffer); } @@ -114,12 +145,12 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con // ---------------------------------------------------------------------- void CameraHandler ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - // TODO + // TODO: Implement image capture command this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } void CameraHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) { - // Append newline to command to send over UART + // Append newline to command to send to PayloadCom Fw::CmdStringArg tempCmd = cmd; tempCmd += "\n"; Fw::Buffer commandBuffer( @@ -127,21 +158,12 @@ void CameraHandler ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, co tempCmd.length() ); - // Send command over output port - Drv::ByteStreamStatus sendStatus = this->out_port_out(0, commandBuffer); + // 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); - - // Log success or failure - if (sendStatus != Drv::ByteStreamStatus::OP_OK) { - this->log_WARNING_HI_CommandError(logCmd); - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); - return; - } - else { - this->log_ACTIVITY_HI_CommandSuccess(logCmd); - } - + this->log_ACTIVITY_HI_CommandSuccess(logCmd); this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } @@ -296,8 +318,8 @@ void CameraHandler ::processProtocolBuffer() { m_fileOpen = true; this->log_ACTIVITY_LO_ImageHeaderReceived(); - // Send ACK to camera after successfully parsing header and opening file - sendAck(); + // 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; @@ -372,8 +394,7 @@ void CameraHandler ::finalizeImageTransfer() { Fw::LogStringArg pathArg(m_currentFilename.c_str()); this->log_ACTIVITY_HI_DataReceived(m_bytes_received, pathArg); - // Send final ACK to camera to confirm complete transfer - sendAck(); + // NOTE: PayloadCom sends ACK automatically - no need to send here // Reset state m_receiving = false; diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 3d3572c5..de690d96 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -1,15 +1,21 @@ module Components { - @ active component that handles camera specific payload capabilities + @ Active component that handles camera-specific payload protocol processing and file saving + @ Receives data from PayloadCom, parses image protocol, saves files active component CameraHandler { - # One async command/port is required for active components - # This should be overridden by the developers with a useful command/port + # Commands @ Type in "snap" to capture an image - async command TAKE_IMAGE() # Command to send data over UART + async command TAKE_IMAGE() - async command SEND_COMMAND(cmd: string) # Command to send data over UART + @ Send command to camera via PayloadCom + async command SEND_COMMAND(cmd: string) - #async command SET_ATTRIBUTES + # Events for protocol processing and file handling + event CommandError(cmd: string) severity warning high format "Failed to send {} command" + + event CommandSuccess(cmd: string) severity activity high format "Command {} sent successfully" + + event DataReceived(data: U8, path: string) severity activity high format "Stored {} bytes of payload data to {}" event ImageHeaderReceived() severity activity low format "Received image header" @@ -23,12 +29,18 @@ module Components { event HeaderParseAttempt(bufSize: U32) severity activity low format "Attempting header parse with {} bytes" + event RawDataDump(byte0: U8, byte1: U8, byte2: U8, byte3: U8, byte4: U8, byte5: U8, byte6: U8, byte7: U8) severity activity low format "Raw: [{x} {x} {x} {x} {x} {x} {x} {x}]" + + # Ports @ Sends command to PayloadCom to be forwarded over UART - output port commandOut: Drv.ByteStreamData + output port commandOut: Drv.ByteStreamData - @ Receives data from PayloadCom over UART, handles image file saving logic + @ Receives data from PayloadCom, handles image protocol parsing and file saving sync input port dataIn: Drv.ByteStreamData + @ Return buffers after processing + output port bufferReturn: Fw.BufferSend + ############################################################################## #### Uncomment the following examples to start customizing your component #### ############################################################################## diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index c4117d64..8419e048 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -2,6 +2,7 @@ // \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 @@ -10,7 +11,7 @@ #include #include #include "FprimeZephyrReference/Components/CameraHandler/CameraHandlerComponentAc.hpp" -#include "Os/File.hpp"> +#include "Os/File.hpp" namespace Components { @@ -33,8 +34,7 @@ class CameraHandler final : public CameraHandlerComponentBase { // ---------------------------------------------------------------------- //! Handler implementation for dataIn - //! - //! Receives data from PayloadCom over UART, handles image file saving logic + //! 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; @@ -45,7 +45,6 @@ class CameraHandler final : public CameraHandlerComponentBase { // ---------------------------------------------------------------------- //! 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 @@ -56,6 +55,42 @@ class CameraHandler final : public CameraHandlerComponentBase { U32 cmdSeq, //!< The command sequence number const Fw::CmdStringArg& cmd) 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); + + // ---------------------------------------------------------------------- + // Member variables + // ---------------------------------------------------------------------- + U8 m_data_file_count = 0; bool m_receiving = false; U32 m_bytes_received = 0; diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp index bb7ff0c1..40d3f3a1 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp @@ -3,12 +3,9 @@ // \author robertpendergrast, moisesmata // \brief cpp file for PayloadCom component implementation class // ====================================================================== -#include "Os/File.hpp" -#include "Fw/Types/Assert.hpp" -#include "Fw/Types/BasicTypes.hpp" #include "FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp" +#include "Fw/Types/BasicTypes.hpp" #include -#include namespace Components { @@ -17,29 +14,16 @@ namespace Components { // ---------------------------------------------------------------------- PayloadCom ::PayloadCom(const char* const compName) - : PayloadComComponentBase(compName), - m_protocolBufferSize(0), - m_fileOpen(false) { - // Initialize protocol buffer to zero - memset(m_protocolBuffer, 0, PROTOCOL_BUFFER_SIZE); -} + : PayloadComComponentBase(compName) {} -PayloadCom ::~PayloadCom() { - // Close file if still open - if (m_fileOpen) { - m_file.close(); - m_fileOpen = false; - } -} +PayloadCom ::~PayloadCom() {} // ---------------------------------------------------------------------- // Handler implementations for typed input ports // ---------------------------------------------------------------------- - -void PayloadCom ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { - +void PayloadCom ::uartDataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { this->log_ACTIVITY_LO_UartReceived(); // Check if we received data successfully @@ -51,8 +35,10 @@ void PayloadCom ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const return; } - this->uartDataOut_out(0, buffer, Drv::ByteStreamStatus::OP_OK); + // 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 @@ -60,38 +46,21 @@ void PayloadCom ::in_port_handler(FwIndexType portNum, Fw::Buffer& buffer, const this->bufferReturn_out(0, buffer); } -// ---------------------------------------------------------------------- -// Handler implementations for commands -// ---------------------------------------------------------------------- - -void PayloadCom ::SEND_COMMAND_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& cmd) { - - // Append newline to command to send over UART - Fw::CmdStringArg tempCmd = cmd; - tempCmd += "\n"; - Fw::Buffer commandBuffer( - reinterpret_cast(const_cast(tempCmd.toChar())), - tempCmd.length() - ); - - // Send command over output port - Drv::ByteStreamStatus sendStatus = this->out_port_out(0, commandBuffer); - - Fw::LogStringArg logCmd(cmd); +void PayloadCom ::commandIn_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { + // Forward command from CameraHandler to UART + // uartForward is ByteStreamSend which returns status + Drv::ByteStreamStatus sendStatus = this->uartForward_out(0, buffer); - // Log success or failure + // Log if send failed (optional) if (sendStatus != Drv::ByteStreamStatus::OP_OK) { - this->log_WARNING_HI_CommandError(logCmd); - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR); - return; - } - else { - this->log_ACTIVITY_HI_CommandSuccess(logCmd); + Fw::LogStringArg logStr("command"); + this->log_WARNING_HI_CommandForwardError(logStr); } - - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); } +// ---------------------------------------------------------------------- +// Helper method implementations +// ---------------------------------------------------------------------- void PayloadCom ::sendAck(){ // Send an acknowledgment over UART @@ -100,8 +69,15 @@ void PayloadCom ::sendAck(){ reinterpret_cast(const_cast(ackMsg)), strlen(ackMsg) ); - this->out_port_out(0, ackBuffer); - + // 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 index 5f4bf093..52f6f248 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp @@ -1,35 +1,30 @@ module Components { - @ Manager for Nicla Vision + @ Barebones UART communication layer for payload (Nicla Vision camera) + @ Handles UART forwarding and ACK handshake, protocol processing done by specific payload handler passive 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 DataReceived( data: U8, path: string) severity activity high format "Stored {} bytes of payload data to {}" - - event ByteReceived( byte: U8) severity activity low format "Received byte: {}" - event UartReceived() severity activity low format "Received UART data" - event BufferAllocationFailed(buffer_size: U32) severity warning high format "Failed to allocate buffer of size {}" - - event RawDataDump(byte0: U8, byte1: U8, byte2: U8, byte3: U8, byte4: U8, byte5: U8, byte6: U8, byte7: U8) severity activity low format "Raw: [{x} {x} {x} {x} {x} {x} {x} {x}]" + 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, handles handshake protocol logic + @ Receives data from the UART, forwards to handler and sends ACK sync input port uartDataIn: Drv.ByteStreamData - @ sends data to the UART, forwards commands and acknowledgement signals + @ 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.ByteStreamSend + output port uartDataOut: Drv.ByteStreamData ############################################################################## #### Uncomment the following examples to start customizing your component #### diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp index 6aa0aa17..9daa2bc2 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.hpp @@ -7,9 +7,6 @@ #ifndef FprimeZephyrReference_PayloadCom_HPP #define FprimeZephyrReference_PayloadCom_HPP -#include -#include -#include "Os/File.hpp" #include "FprimeZephyrReference/Components/PayloadCom/PayloadComComponentAc.hpp" namespace Components { @@ -33,56 +30,22 @@ class PayloadCom final : public PayloadComComponentBase { // Handler implementations for typed input ports // ---------------------------------------------------------------------- - //! Handler implementation for in_port - //! Handler implementation for in_port - void in_port_handler(FwIndexType portNum, //!< The port number - Fw::Buffer& buffer, - const Drv::ByteStreamStatus& status); + //! 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); - private: - // ---------------------------------------------------------------------- - // Handler implementations for commands - // ---------------------------------------------------------------------- - - //! Handler implementation for command SEND_COMMAND - //! - //! TODO - void SEND_COMMAND_cmdHandler(FwOpcodeType opCode, //!< The opcode - U32 cmdSeq, //!< The command sequence number - const Fw::CmdStringArg& cmd); + //! 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 for data accumulation + // Helper methods // ---------------------------------------------------------------------- - //! 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); - //! Send acknowledgment over UART void sendAck(); }; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 9eca4870..03220a72 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -47,6 +47,11 @@ module ReferenceDeployment { stack size Default.STACK_SIZE \ priority 14 + instance cameraHandler: Components.CameraHandler base id 0x10005000 \ + queue size Default.QUEUE_SIZE \ + stack size Default.STACK_SIZE \ + priority 10 + # ---------------------------------------------------------------------- # Queued component instances # ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index d1a196d4..452db92a 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -62,6 +62,7 @@ module ReferenceDeployment { instance payloadBatteryLoadSwitch instance fsSpace instance payload + instance cameraHandler instance peripheralUartDriver instance payloadBufferManager instance cmdSeq @@ -213,12 +214,20 @@ module ReferenceDeployment { } connections PayloadCom { - payload.out_port -> peripheralUartDriver.$send - peripheralUartDriver.$recv -> payload.in_port + # 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 + + # CameraHandler buffer return (after processing data) + cameraHandler.bufferReturn -> payloadBufferManager.bufferSendIn + # UART driver allocates/deallocates from BufferManager peripheralUartDriver.allocate -> payloadBufferManager.bufferGetCallee peripheralUartDriver.deallocate -> payloadBufferManager.bufferSendIn From d8b5c4c558811259e9d2bea3d35f5aa41a049bd9 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Tue, 18 Nov 2025 19:44:11 -0500 Subject: [PATCH 16/34] Swapped components active passive --- .../Components/CameraHandler/CameraHandler.fpp | 6 +++--- FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp | 4 ++-- FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index de690d96..fa6e9cd6 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -1,14 +1,14 @@ module Components { @ Active component that handles camera-specific payload protocol processing and file saving @ Receives data from PayloadCom, parses image protocol, saves files - active component CameraHandler { + passive component CameraHandler { # Commands @ Type in "snap" to capture an image - async command TAKE_IMAGE() + sync command TAKE_IMAGE() @ Send command to camera via PayloadCom - async command SEND_COMMAND(cmd: string) + sync command SEND_COMMAND(cmd: string) # Events for protocol processing and file handling event CommandError(cmd: string) severity warning high format "Failed to send {} command" diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp index 52f6f248..54a7f65a 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp @@ -1,7 +1,7 @@ module Components { @ Barebones UART communication layer for payload (Nicla Vision camera) @ Handles UART forwarding and ACK handshake, protocol processing done by specific payload handler - passive component PayloadCom { + active component PayloadCom { event CommandForwardError(cmd: string) severity warning high format "Failed to send {} command over UART" @@ -12,7 +12,7 @@ module Components { 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 + async input port commandIn: Drv.ByteStreamData @ Receives data from the UART, forwards to handler and sends ACK sync input port uartDataIn: Drv.ByteStreamData diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 03220a72..4ef5e0d6 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -47,7 +47,7 @@ module ReferenceDeployment { stack size Default.STACK_SIZE \ priority 14 - instance cameraHandler: Components.CameraHandler base id 0x10005000 \ + instance payload: Components.PayloadCom base id 0x10005000 \ queue size Default.QUEUE_SIZE \ stack size Default.STACK_SIZE \ priority 10 @@ -114,7 +114,7 @@ module ReferenceDeployment { instance gpioPayloadBatteryLS: Zephyr.ZephyrGpioDriver base id 0x1002A000 - instance payload: Components.PayloadCom base id 0x1002B000 + instance cameraHandler: Components.CameraHandler base id 0x1002B000 instance peripheralUartDriver: Zephyr.ZephyrUartDriver base id 0x1002C000 From 37d62e5a2b01396a29d79527a652e8283fd7f2d0 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Wed, 19 Nov 2025 18:58:55 -0500 Subject: [PATCH 17/34] Data streams in! Just needs to save image properly, look into port calls and doing sync/async --- .../Components/CameraHandler/CameraHandler.cpp | 11 +++-------- .../Components/CameraHandler/CameraHandler.fpp | 3 --- .../Components/PayloadCom/PayloadCom.cpp | 10 ++++++++++ .../Components/PayloadCom/PayloadCom.fpp | 4 ++-- .../ReferenceDeployment/Top/topology.fpp | 3 --- 5 files changed, 15 insertions(+), 16 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index fe2d7250..9feeb04a 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -39,10 +39,7 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con if (m_receiving && m_fileOpen) { handleFileError(); } - // Return buffer even on error - if (buffer.isValid()) { - this->bufferReturn_out(0, buffer); - } + // NOTE: PayloadCom will handle buffer return, not us return; } @@ -75,8 +72,6 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con // Write failed this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write failed")); handleFileError(); - // Return buffer even on error - this->bufferReturn_out(0, buffer); return; } @@ -136,8 +131,8 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con processProtocolBuffer(); } - // Return buffer to allow reuse - this->bufferReturn_out(0, buffer); + // NOTE: Do NOT return buffer here - PayloadCom owns the buffer and will return it + // Returning it twice causes buffer management issues } // ---------------------------------------------------------------------- diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index fa6e9cd6..898b3c0a 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -38,9 +38,6 @@ module Components { @ Receives data from PayloadCom, handles image protocol parsing and file saving sync input port dataIn: Drv.ByteStreamData - @ Return buffers after processing - output port bufferReturn: Fw.BufferSend - ############################################################################## #### Uncomment the following examples to start customizing your component #### ############################################################################## diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp index 40d3f3a1..8b382238 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp @@ -47,6 +47,13 @@ void PayloadCom ::uartDataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, co } 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); @@ -55,6 +62,9 @@ void PayloadCom ::commandIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con 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); } } diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp index 54a7f65a..de69f0c9 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.fpp @@ -12,10 +12,10 @@ module Components { event AckSent() severity activity low format "ACK sent to payload" @ Receives the desired command to forward through the payload UART - async input port commandIn: Drv.ByteStreamData + sync input port commandIn: Drv.ByteStreamData @ Receives data from the UART, forwards to handler and sends ACK - sync input port uartDataIn: Drv.ByteStreamData + async input port uartDataIn: Drv.ByteStreamData @ Sends data to the UART (forwards commands and acknowledgement signals) output port uartForward: Drv.ByteStreamSend diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 452db92a..d957a013 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -225,9 +225,6 @@ module ReferenceDeployment { payload.uartDataOut -> cameraHandler.dataIn cameraHandler.commandOut -> payload.commandIn - # CameraHandler buffer return (after processing data) - cameraHandler.bufferReturn -> payloadBufferManager.bufferSendIn - # UART driver allocates/deallocates from BufferManager peripheralUartDriver.allocate -> payloadBufferManager.bufferGetCallee peripheralUartDriver.deallocate -> payloadBufferManager.bufferSendIn From 87b5383a7a4745e924dc61c90f88d8f954b5b340 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Wed, 19 Nov 2025 20:34:10 -0500 Subject: [PATCH 18/34] take image is a real command --- .../Components/CameraHandler/CameraHandler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 9feeb04a..e3935923 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -140,8 +140,8 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con // ---------------------------------------------------------------------- void CameraHandler ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { - // TODO: Implement image capture command - this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); + 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) { From a7c8800fe8de4f983a5213619e3fe38c002cd1d9 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Wed, 19 Nov 2025 20:43:23 -0500 Subject: [PATCH 19/34] add chunk write event --- .../Components/CameraHandler/CameraHandler.cpp | 3 +++ .../Components/CameraHandler/CameraHandler.fpp | 4 +++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index e3935923..743cd7fa 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -373,6 +373,9 @@ bool CameraHandler ::writeChunkToFile(const U8* data, U32 size) { ptr += toWrite; } + // Log bytes written + this->log_ACTIVITY_LO_ChunkWritten(totalWritten); + return true; } diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 898b3c0a..5d974772 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -23,6 +23,8 @@ module Components { event ImageTransferProgress(received: U32, expected: U32) severity activity low format "Transfer progress: {}/{} bytes" + event ChunkWritten(chunkSize: U32) severity activity low format "Wrote {} bytes to file" + event ImageDataOverflow() severity warning high format "Image data overflow - buffer full" event ProtocolBufferDebug(bufSize: U32, firstByte: U8) severity activity low format "Protocol buffer: {} bytes, first: 0x{x}" @@ -88,4 +90,4 @@ module Components { param set port prmSetOut } -} \ No newline at end of file +} From 2f6ec4965cc2331f49a1f7dac0afaa3cb439ea19 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Fri, 21 Nov 2025 18:58:02 -0500 Subject: [PATCH 20/34] telemetry --- .../CameraHandler/CameraHandler.cpp | 28 +++++++++++++++++++ .../CameraHandler/CameraHandler.fpp | 13 +++++++++ 2 files changed, 41 insertions(+) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 743cd7fa..1b6b155b 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -51,6 +51,12 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con // 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); // DEBUG: Log first 8 bytes ONLY if not receiving (reduces load during transfer) if (!m_receiving && dataSize >= 8) { @@ -77,6 +83,10 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con m_bytes_received += toWrite; + // Emit telemetry after each write + this->tlmWrite_BytesReceived(m_bytes_received); + this->tlmWrite_ExpectedSize(m_expected_size); + // Log progress less frequently to reduce system load (every 512 bytes) if ((m_bytes_received % 512) < 64) { this->log_ACTIVITY_LO_ImageTransferProgress(m_bytes_received, m_expected_size); @@ -312,6 +322,12 @@ void CameraHandler ::processProtocolBuffer() { m_fileOpen = true; this->log_ACTIVITY_LO_ImageHeaderReceived(); + + // 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 @@ -398,6 +414,12 @@ void CameraHandler ::finalizeImageTransfer() { m_receiving = false; m_bytes_received = 0; m_expected_size = 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); } void CameraHandler ::handleFileError() { @@ -415,6 +437,12 @@ void CameraHandler ::handleFileError() { m_bytes_received = 0; m_expected_size = 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); } I32 CameraHandler ::findImageEndMarker(const U8* data, U32 size) { diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 5d974772..23c17dd6 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -33,6 +33,19 @@ module Components { event RawDataDump(byte0: U8, byte1: U8, byte2: U8, byte3: U8, byte4: U8, byte5: U8, byte6: U8, byte7: U8) severity activity low format "Raw: [{x} {x} {x} {x} {x} {x} {x} {x}]" + # 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 + # Ports @ Sends command to PayloadCom to be forwarded over UART output port commandOut: Drv.ByteStreamData From 67d7bd1c4398e31e826c007a8a91b4eb4a0fbdbe Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Fri, 21 Nov 2025 19:10:42 -0500 Subject: [PATCH 21/34] add packet --- .../Components/CameraHandler/CameraHandler.cpp | 2 +- .../Top/ReferenceDeploymentPackets.fppi | 7 +++++++ FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp | 2 +- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 1b6b155b..7e1fdc62 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -205,7 +205,7 @@ void CameraHandler ::processProtocolBuffer() { ); } } - + // Search for anywhere in the buffer (not just at position 0) I32 headerStart = -1; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 04b5552a..eb903859 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -90,6 +90,13 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.powerMonitor.TotalPowerGenerated } + packet CameraDebug id 11 group 5 { + ReferenceDeployment.cameraHandler.BytesReceived + ReferenceDeployment.cameraHandler.ExpectedSize + ReferenceDeployment.cameraHandler.IsReceiving + ReferenceDeployment.cameraHandler.FileOpen + } + } omit { CdhCore.cmdDisp.CommandErrors # Only has one library, no custom versions diff --git a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 4a26c8c9..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 From 2a467e34a754d7778368e812de853eb9e5f52ca9 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Fri, 21 Nov 2025 19:31:23 -0500 Subject: [PATCH 22/34] file error telemetry --- .../Components/CameraHandler/CameraHandler.cpp | 4 ++++ .../Components/CameraHandler/CameraHandler.fpp | 3 +++ .../Components/CameraHandler/CameraHandler.hpp | 1 + .../ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi | 1 + 4 files changed, 9 insertions(+) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 7e1fdc62..884ac0a6 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -429,6 +429,9 @@ void CameraHandler ::handleFileError() { m_fileOpen = false; } + // Increment error counter + m_file_error_count++; + // Log error this->log_WARNING_HI_CommandError(Fw::LogStringArg("File write error")); @@ -443,6 +446,7 @@ void CameraHandler ::handleFileError() { 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) { diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 23c17dd6..92848f82 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -45,6 +45,9 @@ module Components { @ Whether file is currently open for writing telemetry FileOpen: bool + + @ Total number of file errors encountered + telemetry FileErrorCount: U32 # Ports @ Sends command to PayloadCom to be forwarded over UART diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index 8419e048..f7f41225 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -94,6 +94,7 @@ class CameraHandler final : public CameraHandlerComponentBase { U8 m_data_file_count = 0; bool m_receiving = false; U32 m_bytes_received = 0; + U32 m_file_error_count = 0; // Track total file errors U8 m_lineBuffer[128]; size_t m_lineIndex = 0; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index eb903859..4e5892e8 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -95,6 +95,7 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.cameraHandler.ExpectedSize ReferenceDeployment.cameraHandler.IsReceiving ReferenceDeployment.cameraHandler.FileOpen + ReferenceDeployment.cameraHandler.FileErrorCount } } omit { From 1d60e93b21dfe22c397a10402bddb14ceb5a5809 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Fri, 21 Nov 2025 19:33:37 -0500 Subject: [PATCH 23/34] comment out events --- .../Components/CameraHandler/CameraHandler.cpp | 2 +- FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 884ac0a6..9b7d3db2 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -390,7 +390,7 @@ bool CameraHandler ::writeChunkToFile(const U8* data, U32 size) { } // Log bytes written - this->log_ACTIVITY_LO_ChunkWritten(totalWritten); + // this->log_ACTIVITY_LO_ChunkWritten(totalWritten); return true; } diff --git a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp index 8b382238..74be6bec 100644 --- a/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp +++ b/FprimeZephyrReference/Components/PayloadCom/PayloadCom.cpp @@ -24,7 +24,7 @@ PayloadCom ::~PayloadCom() {} // ---------------------------------------------------------------------- void PayloadCom ::uartDataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, const Drv::ByteStreamStatus& status) { - this->log_ACTIVITY_LO_UartReceived(); + // this->log_ACTIVITY_LO_UartReceived(); // Check if we received data successfully if (status != Drv::ByteStreamStatus::OP_OK) { @@ -83,7 +83,7 @@ void PayloadCom ::sendAck(){ Drv::ByteStreamStatus sendStatus = this->uartForward_out(0, ackBuffer); if (sendStatus == Drv::ByteStreamStatus::OP_OK) { - this->log_ACTIVITY_LO_AckSent(); + // this->log_ACTIVITY_LO_AckSent(); } else { Fw::LogStringArg logStr("ACK"); this->log_WARNING_HI_CommandForwardError(logStr); From cfcb2aa6848e420a1f3f71f952abde6533d4bc54 Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Fri, 21 Nov 2025 19:39:24 -0500 Subject: [PATCH 24/34] add images saved tlm --- .../Components/CameraHandler/CameraHandler.cpp | 4 ++++ .../Components/CameraHandler/CameraHandler.fpp | 3 +++ .../Components/CameraHandler/CameraHandler.hpp | 1 + .../ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi | 1 + 4 files changed, 9 insertions(+) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 9b7d3db2..aab9b5c1 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -404,6 +404,9 @@ void CameraHandler ::finalizeImageTransfer() { m_file.close(); m_fileOpen = false; + // Increment success counter + m_images_saved++; + // Log success Fw::LogStringArg pathArg(m_currentFilename.c_str()); this->log_ACTIVITY_HI_DataReceived(m_bytes_received, pathArg); @@ -420,6 +423,7 @@ void CameraHandler ::finalizeImageTransfer() { 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() { diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 92848f82..7351de6b 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -48,6 +48,9 @@ module Components { @ 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 diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index f7f41225..9c3cfcf6 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -95,6 +95,7 @@ class CameraHandler final : public CameraHandlerComponentBase { bool m_receiving = 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 U8 m_lineBuffer[128]; size_t m_lineIndex = 0; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 4e5892e8..605164a1 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -96,6 +96,7 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.cameraHandler.IsReceiving ReferenceDeployment.cameraHandler.FileOpen ReferenceDeployment.cameraHandler.FileErrorCount + ReferenceDeployment.cameraHandler.ImagesSaved } } omit { From 09ebe80895598609ef7aaa505adf3fa1482ade0c Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Sun, 23 Nov 2025 16:27:18 -0500 Subject: [PATCH 25/34] Sarah's Commit --- .../CameraHandler/CameraHandler.cpp | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index aab9b5c1..d4ba0657 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -72,6 +72,22 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con // 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 + U32 extraBytes = dataSize - toWrite; + if (extraBytes > 0) { + const U8* extraData = data + toWrite; + if (accumulateProtocolData(extraData, extraBytes)) { + processProtocolBuffer(); + } + } + } // Write chunk to file if (!writeChunkToFile(data, toWrite)) { @@ -92,21 +108,6 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con this->log_ACTIVITY_LO_ImageTransferProgress(m_bytes_received, m_expected_size); } - // 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 - U32 extraBytes = dataSize - toWrite; - if (extraBytes > 0) { - const U8* extraData = data + toWrite; - if (accumulateProtocolData(extraData, extraBytes)) { - processProtocolBuffer(); - } - } - } } else { // Not receiving image - accumulate protocol data From aaa4cca5b134fe6ff09d0a8caf95d16ba623b64f Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Sat, 29 Nov 2025 15:42:46 -0500 Subject: [PATCH 26/34] Add return so that we don't write after saving --- .../Components/CameraHandler/CameraHandler.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index d4ba0657..dc54d0f1 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -79,7 +79,7 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con finalizeImageTransfer(); // If there's extra data after the image (e.g., or next header), - // push it to protocol buffer + // push it to protocol buffer for processing U32 extraBytes = dataSize - toWrite; if (extraBytes > 0) { const U8* extraData = data + toWrite; @@ -87,6 +87,9 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con processProtocolBuffer(); } } + + // Transfer is complete - don't try to write anything more + return; } // Write chunk to file From ed7ad4b4aaa3b37a65e0ddbf6275e1b374c4c804 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Sat, 29 Nov 2025 16:07:59 -0500 Subject: [PATCH 27/34] Update and Streamline events emitted --- .../CameraHandler/CameraHandler.cpp | 60 +++++++------------ .../CameraHandler/CameraHandler.fpp | 24 +++----- .../CameraHandler/CameraHandler.hpp | 1 + 3 files changed, 31 insertions(+), 54 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index dc54d0f1..bef936b2 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -58,14 +58,6 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con this->tlmWrite_IsReceiving(m_receiving); this->tlmWrite_FileOpen(m_fileOpen); - // DEBUG: Log first 8 bytes ONLY if not receiving (reduces load during transfer) - if (!m_receiving && dataSize >= 8) { - this->log_ACTIVITY_LO_RawDataDump( - data[0], data[1], data[2], data[3], - data[4], data[5], data[6], data[7] - ); - } - if (m_receiving && m_fileOpen) { // Currently receiving image data - write directly to file @@ -106,9 +98,20 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con this->tlmWrite_BytesReceived(m_bytes_received); this->tlmWrite_ExpectedSize(m_expected_size); - // Log progress less frequently to reduce system load (every 512 bytes) - if ((m_bytes_received % 512) < 64) { - this->log_ACTIVITY_LO_ImageTransferProgress(m_bytes_received, 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 { @@ -136,11 +139,6 @@ void CameraHandler ::dataIn_handler(FwIndexType portNum, Fw::Buffer& buffer, con } } - // Log what we have in protocol buffer for debugging - if (m_protocolBufferSize > 0) { - this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); - } - // Process protocol buffer to detect image headers/commands processProtocolBuffer(); } @@ -197,19 +195,6 @@ bool CameraHandler ::accumulateProtocolData(const U8* data, U32 size) { void CameraHandler ::processProtocolBuffer() { // Protocol: [4-byte little-endian uint32][image data] - // Log parse attempt for debugging - if (m_protocolBufferSize > 0) { - this->log_ACTIVITY_LO_HeaderParseAttempt(m_protocolBufferSize); - - // Dump first 8 bytes for debugging - if (m_protocolBufferSize >= 8) { - this->log_ACTIVITY_LO_RawDataDump( - m_protocolBuffer[0], m_protocolBuffer[1], m_protocolBuffer[2], m_protocolBuffer[3], - m_protocolBuffer[4], m_protocolBuffer[5], m_protocolBuffer[6], m_protocolBuffer[7] - ); - } - } - // Search for anywhere in the buffer (not just at position 0) I32 headerStart = -1; @@ -244,9 +229,6 @@ void CameraHandler ::processProtocolBuffer() { U32 remaining = m_protocolBufferSize - static_cast(headerStart); memmove(m_protocolBuffer, &m_protocolBuffer[headerStart], remaining); m_protocolBufferSize = remaining; - - // Log that we found and moved the header - this->log_ACTIVITY_LO_ProtocolBufferDebug(m_protocolBufferSize, m_protocolBuffer[0]); } // Now check if we have the complete header (28 bytes minimum) @@ -303,9 +285,7 @@ void CameraHandler ::processProtocolBuffer() { m_receiving = true; m_bytes_received = 0; m_expected_size = imageSize; - - // Log the extracted size - this->log_ACTIVITY_HI_ImageSizeExtracted(imageSize); + m_lastMilestone = 0; // Reset milestone tracking for new transfer // Generate filename - save to root filesystem char filename[64]; @@ -325,7 +305,9 @@ void CameraHandler ::processProtocolBuffer() { } m_fileOpen = true; - this->log_ACTIVITY_LO_ImageHeaderReceived(); + + // Log transfer started event + this->log_ACTIVITY_HI_ImageTransferStarted(imageSize); // Emit telemetry after opening file this->tlmWrite_BytesReceived(m_bytes_received); @@ -411,9 +393,9 @@ void CameraHandler ::finalizeImageTransfer() { // Increment success counter m_images_saved++; - // Log success + // Log transfer complete event with path and size Fw::LogStringArg pathArg(m_currentFilename.c_str()); - this->log_ACTIVITY_HI_DataReceived(m_bytes_received, pathArg); + this->log_ACTIVITY_HI_ImageTransferComplete(pathArg, m_bytes_received); // NOTE: PayloadCom sends ACK automatically - no need to send here @@ -421,6 +403,7 @@ void CameraHandler ::finalizeImageTransfer() { m_receiving = false; m_bytes_received = 0; m_expected_size = 0; + m_lastMilestone = 0; // Emit telemetry after finalizing this->tlmWrite_BytesReceived(m_bytes_received); @@ -447,6 +430,7 @@ void CameraHandler ::handleFileError() { m_receiving = false; m_bytes_received = 0; m_expected_size = 0; + m_lastMilestone = 0; clearProtocolBuffer(); // Emit telemetry after error handling diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 7351de6b..2a31dd37 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -10,28 +10,20 @@ module Components { @ Send command to camera via PayloadCom sync command SEND_COMMAND(cmd: string) - # Events for protocol processing and file handling + # 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" - event DataReceived(data: U8, path: string) severity activity high format "Stored {} bytes of payload data to {}" + # 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" - event ImageHeaderReceived() severity activity low format "Received image header" + @ Emitted at 25%, 50%, 75% progress milestones + event ImageTransferProgress(percent: U8, received: U32, expected: U32) severity activity high format "Image transfer {}% complete ({}/{} bytes)" - event ImageSizeExtracted(imageSize: U32) severity activity high format "Image size from header: {} bytes" - - event ImageTransferProgress(received: U32, expected: U32) severity activity low format "Transfer progress: {}/{} bytes" - - event ChunkWritten(chunkSize: U32) severity activity low format "Wrote {} bytes to file" - - event ImageDataOverflow() severity warning high format "Image data overflow - buffer full" - - event ProtocolBufferDebug(bufSize: U32, firstByte: U8) severity activity low format "Protocol buffer: {} bytes, first: 0x{x}" - - event HeaderParseAttempt(bufSize: U32) severity activity low format "Attempting header parse with {} bytes" - - event RawDataDump(byte0: U8, byte1: U8, byte2: U8, byte3: U8, byte4: U8, byte5: U8, byte6: U8, byte7: U8) severity activity low format "Raw: [{x} {x} {x} {x} {x} {x} {x} {x}]" + @ Emitted when image transfer completes successfully + event ImageTransferComplete(path: string, $size: U32) severity activity high format "Image saved: {} ({} bytes)" # Telemetry for debugging image transfer state @ Number of bytes received so far in current image transfer diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index 9c3cfcf6..3583707b 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -123,6 +123,7 @@ class CameraHandler final : public CameraHandlerComponentBase { 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 From cde9bc9d0ef8484c0447828d1cfe615ff1286d0c Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Sat, 29 Nov 2025 16:16:31 -0500 Subject: [PATCH 28/34] new commands --- .../Components/CameraHandler/CameraHandler.cpp | 10 ++++++++++ .../Components/CameraHandler/CameraHandler.fpp | 6 ++++++ .../Components/CameraHandler/CameraHandler.hpp | 8 ++++++++ 3 files changed, 24 insertions(+) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index bef936b2..6800334a 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -156,6 +156,16 @@ void CameraHandler ::TAKE_IMAGE_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { SEND_COMMAND_cmdHandler(opCode, cmdSeq, Fw::CmdStringArg(takeImageCmd)); } +void CameraHandler ::PING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { + 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; diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 2a31dd37..46dab713 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp @@ -7,6 +7,12 @@ module Components { @ Type in "snap" to capture an image sync command TAKE_IMAGE() + @ 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) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index 3583707b..30de58d6 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -55,6 +55,14 @@ class CameraHandler final : public CameraHandlerComponentBase { 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; + + void SET_IMAGE_QUALITY_cmdHandler(FwOpcodeType opCode, //!< The opcode + U32 cmdSeq //!< The command sequence number + ) override; + // ---------------------------------------------------------------------- // Helper methods for protocol processing // ---------------------------------------------------------------------- From ef8f2835e761da98c55e9080983017171737530e Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Sat, 29 Nov 2025 19:14:56 -0500 Subject: [PATCH 29/34] DONE --- .../CameraHandler/CameraHandler.cpp | 44 +- .../CameraHandler/CameraHandler.fpp | 9 +- .../CameraHandler/CameraHandler.hpp | 15 +- micro-python/camera-main.py | 392 ++++++++++++++++++ 4 files changed, 446 insertions(+), 14 deletions(-) create mode 100644 micro-python/camera-main.py diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 6800334a..3bff0a7d 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/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(command[i])) { + return false; + } + } + + return true; +} + } // namespace Components diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp index 46dab713..abe3a63d 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.fpp +++ b/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 diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index 30de58d6..c45931ba 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/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 "" 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("") 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) diff --git a/micro-python/camera-main.py b/micro-python/camera-main.py new file mode 100644 index 00000000..65fc2e1f --- /dev/null +++ b/micro-python/camera-main.py @@ -0,0 +1,392 @@ +# 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. + +import sensor, time +from pyb import UART, LED +import struct + +# --- 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) +image_count = 0 + +# Initialize camera with error handling for standalone operation +try: + sensor.reset() + time.sleep_ms(100) # Brief pause after reset + 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) +except Exception as e: + print("ERROR: Camera initialization failed:", e) + # Flash red LED to indicate camera init failure + for _ in range(5): + red.on(); time.sleep_ms(100); red.off(); time.sleep_ms(100) + raise # Re-raise to stop execution if camera can't initialize + + +# --- Utility functions --- + +def write_all(uart_obj, data, write_timeout_ms=2000): + """ + 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() + 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: + return False + return True + +def is_ack(ack_bytes): + """ + 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=3000): + """ + 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) + # We will use uart.readline() which returns bytes up to newline (or None on timeout) + while time.ticks_diff(deadline, time.ticks_ms()) > 0: + 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) + # timed out + return None + +def send_image_protocol(jpeg_bytes): + """ + Send JPEG image bytes using protocol with ACK handshake: + [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") + red.on(); time.sleep_ms(200); red.off() + return False + + # Wait for ACK after header + ack = wait_for_ack(total_timeout_ms=3000) + if not ack: + print("ERROR: No ACK after header") + red.on(); time.sleep_ms(300); red.off() + 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") + red.on(); time.sleep_ms(300); red.off() + 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)) + red.on(); time.sleep_ms(300); red.off() + return False + except Exception as e: + print("ERROR: sending JPEG data:", e) + red.on(); time.sleep_ms(300); red.off() + return False + + # Send footer + if not write_all(uart, b""): + print("ERROR: footer write failed") + red.on(); time.sleep_ms(300); red.off() + return False + + # Final ACK + if wait_for_ack(total_timeout_ms=3000): + # flash green LED to show success + for _ in range(3): + green.on(); time.sleep_ms(100); green.off(); time.sleep_ms(100) + return True + else: + print("ERROR: No final ACK") + red.on(); time.sleep_ms(300); red.off() + return False + +def ping_handler(): + """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 + green.on(); time.sleep_ms(50); green.off() + 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(): + 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) + 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) + red.on(); time.sleep_ms(200); red.off() + state = STATE_ERROR + return + else: + print("ERROR: Failed to get JPEG data from image:", e) + red.on(); time.sleep_ms(200); red.off() + state = STATE_ERROR + return + + if not jpeg_bytes or len(jpeg_bytes) == 0: + print("ERROR: Failed to get JPEG data from image") + red.on(); time.sleep_ms(200); red.off() + 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) + red.on(); time.sleep_ms(200); red.off() + 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) +print("Ready for commands...") + +heartbeat_ms = 1000 +last_hb = time.ticks_ms() + +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 + green.on(); time.sleep_ms(80); green.off() + print("Command received: '{}'".format(text)) + + # Commands + cmd = text.lower() + handler = COMMANDS.get(cmd) + if handler == None: + print("Unknown command: '{}'".format(text)) + # Unknown commands + pass + else: + try: + handler() + except Exception as e: + print("ERROR in handler:", e) + red.on(); time.sleep_ms(200); red.off() + state = STATE_ERROR + pass + + elif state == STATE_ERROR: + # TODO: How should we handle errors? + print("Recovering from error...") + time.sleep_ms(500) + state = STATE_IDLE + + # Idle heartbeat + if time.ticks_diff(time.ticks_ms(), last_hb) >= heartbeat_ms: + try: + blue.toggle() + except Exception: + # some platforms may not have toggle; emulate + blue.on(); time.sleep_ms(20); blue.off() + last_hb = time.ticks_ms() + + # yield tiny amount of CPU + time.sleep_ms(10) \ No newline at end of file From 7bd231469fa6480aca5bf3953349f382f75ff400 Mon Sep 17 00:00:00 2001 From: Robert Pendergrast <121700465+RobertPendergrast@users.noreply.github.com> Date: Sun, 30 Nov 2025 12:28:42 -0500 Subject: [PATCH 30/34] Revise CameraHandler documentation Updated documentation for the CameraHandler component, including usage examples, port descriptions, commands, and requirements. --- .../Components/CameraHandler/docs/sdd.md | 42 +++++++------------ 1 file changed, 16 insertions(+), 26 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md index 8eb34917..caee191a 100644 --- a/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md +++ b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md @@ -1,42 +1,30 @@ # Components::CameraHandler -passive component that handles camera specific payload capabilities +Passive component that handles camera specific payload capabilities. + - Taking Images + - Pinging ## Usage Examples -Add usage examples here - -### Diagrams -Add diagrams here +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 -And the typical usage of the component here - -## Class Diagram -Add a class diagram here +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 | -|---|---| -|---|---| - -## Sequence Diagrams -Add sequence diagrams here - -## Parameters -| 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 | @@ -57,10 +45,12 @@ Add unit test descriptions in the chart below ## 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 | \ No newline at end of file +|---| Initial Draft | From ca82349422f823dbbc2403b4b4a7c26b9088c07b Mon Sep 17 00:00:00 2001 From: Robert Pendergrast <121700465+RobertPendergrast@users.noreply.github.com> Date: Sun, 30 Nov 2025 12:30:06 -0500 Subject: [PATCH 31/34] Format documentation for consistency --- .../Components/CameraHandler/docs/sdd.md | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md index caee191a..6daa3604 100644 --- a/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md +++ b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md @@ -12,19 +12,19 @@ Prior to taking a picture, the payload power loadswitch must be activated. Then ## Port Descriptions | Name | Description | -|commandOut|Command to forward to the PayloadCom component| -|dataIn|Data received from the PayloadCom component| +| 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| +| 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| +| 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 | @@ -45,10 +45,10 @@ Add unit test descriptions in the chart below ## 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| +| 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 | From 15cbf7b3aa93e6e4c27ca376f9b241e13682e45b Mon Sep 17 00:00:00 2001 From: Robert Pendergrast <121700465+RobertPendergrast@users.noreply.github.com> Date: Sun, 30 Nov 2025 12:31:43 -0500 Subject: [PATCH 32/34] Format markdown tables for better readability --- .../Components/CameraHandler/docs/sdd.md | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md index 6daa3604..90c42299 100644 --- a/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md +++ b/FprimeZephyrReference/Components/CameraHandler/docs/sdd.md @@ -11,19 +11,22 @@ The camera handler can be commanded to take an image, after which it will forwar 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 | +| 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 | +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 | +| 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 @@ -45,6 +48,7 @@ Add unit test descriptions in the chart below ## 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 | From c7d190d27ee2ee560d4e7632bc5cb0cbc99b878a Mon Sep 17 00:00:00 2001 From: Wesley Maa Date: Sun, 30 Nov 2025 13:15:05 -0500 Subject: [PATCH 33/34] fmt --- micro-python/camera-main.py | 206 ++++++++++++++++++++---------------- micro-python/pyproject.toml | 65 ++++++++++++ 2 files changed, 182 insertions(+), 89 deletions(-) create mode 100644 micro-python/pyproject.toml diff --git a/micro-python/camera-main.py b/micro-python/camera-main.py index 65fc2e1f..b1c95ad5 100644 --- a/micro-python/camera-main.py +++ b/micro-python/camera-main.py @@ -1,23 +1,62 @@ -# 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. - -import sensor, time -from pyb import UART, LED +"""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 +red = LED(1) # error indicator green = LED(2) # activity (command received) -blue = LED(3) # idle heartbeat +blue = LED(3) # idle heartbeat # --- STATE MACHINE --- STATE_IDLE = 0 @@ -34,54 +73,39 @@ # --- Camera Setup --- # Add delay for hardware to stabilize when running standalone -time.sleep_ms(500) +time.sleep_ms(500) # type: ignore[attr-defined] image_count = 0 # Initialize camera with error handling for standalone operation try: sensor.reset() - time.sleep_ms(100) # Brief pause after 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), + # 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) + 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): - red.on(); time.sleep_ms(100); red.off(); time.sleep_ms(100) + blink_led(red, 100) + time.sleep_ms(100) # type: ignore[attr-defined] raise # Re-raise to stop execution if camera can't initialize -# --- Utility functions --- +# --- Communication functions --- -def write_all(uart_obj, data, write_timeout_ms=2000): - """ - 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() - 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: - return False - return True -def is_ack(ack_bytes): - """ - Decide whether the received bytes qualify as an ACK. +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" """ @@ -105,14 +129,15 @@ def is_ack(ack_bytes): return True return False -def wait_for_ack(total_timeout_ms=3000): - """ - Wait for an ACK line from UART up to total_timeout_ms. + +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) + 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: + while time.ticks_diff(deadline, time.ticks_ms()) > 0: # type: ignore[attr-defined] try: line = uart.readline() except Exception: @@ -129,18 +154,19 @@ def wait_for_ack(total_timeout_ms=3000): return line # not a recognized ack -> continue reading until timeout (ignore other chatter) # small sleep to yield CPU - time.sleep_ms(10) + time.sleep_ms(10) # type: ignore[attr-defined] # timed out return None -def send_image_protocol(jpeg_bytes): - """ - Send JPEG image bytes using protocol with ACK handshake: - [4-byte LE uint32][data chunks with ACK] + +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 @@ -158,14 +184,14 @@ def send_image_protocol(jpeg_bytes): # Send header if not write_all(uart, header): print("ERROR: header write failed/timeout") - red.on(); time.sleep_ms(200); red.off() + 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") - red.on(); time.sleep_ms(300); red.off() + blink_led(red, 300) return False # Send image data in chunks with ACK handshake @@ -176,13 +202,13 @@ def send_image_protocol(jpeg_bytes): 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] + 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") - red.on(); time.sleep_ms(300); red.off() + blink_led(red, 300) return False bytes_sent += len(chunk) offset += len(chunk) @@ -190,39 +216,41 @@ def send_image_protocol(jpeg_bytes): ack = wait_for_ack(total_timeout_ms=2000) if not ack: print("ERROR: No ACK after chunk (bytes_sent={})".format(bytes_sent)) - red.on(); time.sleep_ms(300); red.off() + blink_led(red, 300) return False except Exception as e: print("ERROR: sending JPEG data:", e) - red.on(); time.sleep_ms(300); red.off() + blink_led(red, 300) return False # Send footer if not write_all(uart, b""): print("ERROR: footer write failed") - red.on(); time.sleep_ms(300); red.off() + 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): - green.on(); time.sleep_ms(100); green.off(); time.sleep_ms(100) + blink_led(green, 100) + time.sleep_ms(100) # type: ignore[attr-defined] return True else: print("ERROR: No final ACK") - red.on(); time.sleep_ms(300); red.off() + blink_led(red, 300) return False -def ping_handler(): - """Respond to ping command with pong message""" + +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 - green.on(); time.sleep_ms(50); green.off() + blink_led(green, duration_ms=50) return True else: print("ERROR: Failed to send PONG response") @@ -231,28 +259,26 @@ def ping_handler(): print("ERROR in ping_handler():", e) return False -def snap_handler(): + +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) + time.sleep_ms(50) # type: ignore[attr-defined] img = sensor.snapshot() - - jpeg_params = { - 'quality': QUALITY, - 'encode_for_ide': False - } + + 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: @@ -263,22 +289,22 @@ def snap_handler(): 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_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) - red.on(); time.sleep_ms(200); red.off() + blink_led(red, 200) state = STATE_ERROR return else: print("ERROR: Failed to get JPEG data from image:", e) - red.on(); time.sleep_ms(200); red.off() + 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") - red.on(); time.sleep_ms(200); red.off() + blink_led(red, 200) state = STATE_ERROR return @@ -297,9 +323,10 @@ def snap_handler(): except Exception as e: print("ERROR in snap():", e) - red.on(); time.sleep_ms(200); red.off() + blink_led(red, 200) state = STATE_ERROR + # --- Main Loop --- # Brief startup logging (no noisy startup protocol messages) @@ -312,11 +339,11 @@ def snap_handler(): except Exception: pass -time.sleep_ms(100) +time.sleep_ms(100) # type: ignore[attr-defined] print("Ready for commands...") heartbeat_ms = 1000 -last_hb = time.ticks_ms() +last_hb = time.ticks_ms() # type: ignore[attr-defined] COMMANDS = { "snap": snap_handler, @@ -327,6 +354,7 @@ def snap_handler(): if DEBUG: from pyb import USB_VCP + try: usb = USB_VCP() usb.setinterrupt(-1) @@ -339,7 +367,7 @@ def snap_handler(): if state == STATE_IDLE: try: msg = uart.readline() - if DEBUG: + if DEBUG: msg = usb.readline() except Exception: msg = None @@ -354,39 +382,39 @@ def snap_handler(): if text: # blink green LED for received command - green.on(); time.sleep_ms(80); green.off() + blink_led(green, 80) print("Command received: '{}'".format(text)) # Commands cmd = text.lower() handler = COMMANDS.get(cmd) - if handler == None: + if handler is None: print("Unknown command: '{}'".format(text)) # Unknown commands pass else: - try: + try: handler() except Exception as e: print("ERROR in handler:", e) - red.on(); time.sleep_ms(200); red.off() + 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) + time.sleep_ms(500) # type: ignore[attr-defined] state = STATE_IDLE # Idle heartbeat - if time.ticks_diff(time.ticks_ms(), last_hb) >= heartbeat_ms: + 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 - blue.on(); time.sleep_ms(20); blue.off() - last_hb = time.ticks_ms() + blink_led(blue, 20) + last_hb = time.ticks_ms() # type: ignore[attr-defined] # yield tiny amount of CPU - time.sleep_ms(10) \ No newline at end of file + 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" From 323d8f46a2b4a14dd74b355c8af5a5d4b35592e4 Mon Sep 17 00:00:00 2001 From: robertpendergrast Date: Thu, 4 Dec 2025 18:52:19 -0500 Subject: [PATCH 34/34] Idk if this works might have to revert --- .../CameraHandler/CameraHandler.cpp | 4 ++- .../CameraHandler/CameraHandler.hpp | 5 +++ .../ReferenceDeployment/Main.cpp | 3 ++ .../Top/ReferenceDeploymentPackets.fppi | 14 +++++++++ .../Top/ReferenceDeploymentTopology.cpp | 3 ++ .../Top/ReferenceDeploymentTopologyDefs.hpp | 2 ++ .../ReferenceDeployment/Top/instances.fpp | 31 +++++++++++++++++++ .../ReferenceDeployment/Top/topology.fpp | 24 ++++++++++++++ .../project/config/AcConstants.fpp | 2 +- 9 files changed, 86 insertions(+), 2 deletions(-) diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp index 3bff0a7d..a296e426 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.cpp @@ -314,7 +314,9 @@ void CameraHandler ::processProtocolBuffer() { // Generate filename - save to root filesystem char filename[64]; - snprintf(filename, sizeof(filename), "/img_%03d.jpg", m_data_file_count++); + // 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 diff --git a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp index c45931ba..59953715 100644 --- a/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp +++ b/FprimeZephyrReference/Components/CameraHandler/CameraHandler.hpp @@ -28,6 +28,10 @@ class CameraHandler final : public CameraHandlerComponentBase { //! Destroy CameraHandler object ~CameraHandler(); + void configure(U32 cam_num) { + this->cam_number = cam_num; + } + private: // ---------------------------------------------------------------------- // Handler implementations for typed input ports @@ -104,6 +108,7 @@ class CameraHandler final : public CameraHandlerComponentBase { 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; diff --git a/FprimeZephyrReference/ReferenceDeployment/Main.cpp b/FprimeZephyrReference/ReferenceDeployment/Main.cpp index 4241b82a..d6d7f462 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Main.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Main.cpp @@ -15,6 +15,7 @@ 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)); @@ -40,6 +41,8 @@ int main(int argc, char* argv[]) { // 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); diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 605164a1..ad805999 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -21,6 +21,8 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.EmptyBuffs payloadBufferManager.NoBuffs payloadBufferManager.EmptyBuffs + payloadBufferManager2.NoBuffs + payloadBufferManager2.EmptyBuffs ReferenceDeployment.rateGroup10Hz.RgCycleSlips ReferenceDeployment.rateGroup1Hz.RgCycleSlips } @@ -36,6 +38,9 @@ telemetry packets ReferenceDeploymentPackets { payloadBufferManager.TotalBuffs payloadBufferManager.CurrBuffs payloadBufferManager.HiBuffs + payloadBufferManager2.TotalBuffs + payloadBufferManager2.CurrBuffs + payloadBufferManager2.HiBuffs CdhCore.tlmSend.SendLevel } @@ -99,6 +104,15 @@ telemetry packets ReferenceDeploymentPackets { 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 5299e185..0a96c23b 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -117,6 +117,9 @@ void setupTopology(const TopologyState& state) { 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 a3eacee6..9bc496d8 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -77,6 +77,8 @@ struct TopologyState { 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 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 4ef5e0d6..b14fc6a9 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -52,6 +52,11 @@ module ReferenceDeployment { 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 # ---------------------------------------------------------------------- @@ -167,4 +172,30 @@ module ReferenceDeployment { 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 d957a013..f6e0fd54 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -62,9 +62,13 @@ module ReferenceDeployment { 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 @@ -158,8 +162,10 @@ module ReferenceDeployment { rateGroup10Hz.RateGroupMemberOut[1] -> ComCcsdsUart.aggregator.timeout rateGroup10Hz.RateGroupMemberOut[2] -> ComCcsds.aggregator.timeout 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 @@ -177,6 +183,7 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[11] -> FileHandling.fileDownlink.Run rateGroup1Hz.RateGroupMemberOut[12] -> startupManager.run rateGroup1Hz.RateGroupMemberOut[13] -> powerMonitor.run + rateGroup1Hz.RateGroupMemberOut[14] -> payloadBufferManager2.schedIn } @@ -230,6 +237,23 @@ module ReferenceDeployment { 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