diff --git a/.codespell-ignore-words.txt b/.codespell-ignore-words.txt new file mode 100644 index 0000000..f5f10d4 --- /dev/null +++ b/.codespell-ignore-words.txt @@ -0,0 +1 @@ +comIn diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index a9a9681..e6514ca 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -12,6 +12,7 @@ repos: rev: v2.4.1 hooks: - id: codespell + args: [--ignore-words=.codespell-ignore-words.txt] - repo: https://github.com/pre-commit/mirrors-clang-format rev: v20.1.8 diff --git a/FprimeZephyrReference/CMakeLists.txt b/FprimeZephyrReference/CMakeLists.txt index 70c1681..95ee3b9 100644 --- a/FprimeZephyrReference/CMakeLists.txt +++ b/FprimeZephyrReference/CMakeLists.txt @@ -1,6 +1,8 @@ # This CMake file is intended to register project-wide objects. # This allows for reuse between deployments, or other projects. +# Keep me first add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/project/config") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Components") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ComCcsdsUart/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ReferenceDeployment/") diff --git a/FprimeZephyrReference/ComCcsdsUart/CMakeLists.txt b/FprimeZephyrReference/ComCcsdsUart/CMakeLists.txt new file mode 100644 index 0000000..a95400b --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsUart/CMakeLists.txt @@ -0,0 +1,12 @@ + +register_fprime_module( + EXCLUDE_FROM_ALL + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/ComCcsds.fpp" + HEADERS + "${CMAKE_CURRENT_LIST_DIR}/SubtopologyTopologyDefs.hpp" + "${CMAKE_CURRENT_LIST_DIR}/PingEntries.hpp" + DEPENDS + Svc_Subtopologies_ComCcsds_ComCcsdsConfig + INTERFACE +) diff --git a/FprimeZephyrReference/ComCcsdsUart/ComCcsds.fpp b/FprimeZephyrReference/ComCcsdsUart/ComCcsds.fpp new file mode 100644 index 0000000..293f630 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsUart/ComCcsds.fpp @@ -0,0 +1,203 @@ +module ComCcsdsUart { + + # ComPacket Queue enum for queue types + enum Ports_ComPacketQueue { + EVENTS, + TELEMETRY + } + + enum Ports_ComBufferQueue { + FILE + } + + # ---------------------------------------------------------------------- + # Active Components + # ---------------------------------------------------------------------- + instance comQueue: Svc.ComQueue base id ComCcsdsConfig.BASE_ID_UART + 0x00000 \ + queue size ComCcsdsConfig.QueueSizes.comQueue \ + stack size ComCcsdsConfig.StackSizes.comQueue \ + priority ComCcsdsConfig.Priorities.comQueue \ + { + phase Fpp.ToCpp.Phases.configComponents """ + using namespace ComCcsdsUart; + Svc::ComQueue::QueueConfigurationTable configurationTableUart; + + // Events (highest-priority) + configurationTableUart.entries[Ports_ComPacketQueue::EVENTS].depth = ComCcsdsConfig::QueueDepths::events; + configurationTableUart.entries[Ports_ComPacketQueue::EVENTS].priority = ComCcsdsConfig::QueuePriorities::events; + + // Telemetry + configurationTableUart.entries[Ports_ComPacketQueue::TELEMETRY].depth = ComCcsdsConfig::QueueDepths::tlm; + configurationTableUart.entries[Ports_ComPacketQueue::TELEMETRY].priority = ComCcsdsConfig::QueuePriorities::tlm; + + // File Downlink Queue (buffer queue using NUM_CONSTANTS offset) + configurationTableUart.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].depth = ComCcsdsConfig::QueueDepths::file; + configurationTableUart.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].priority = ComCcsdsConfig::QueuePriorities::file; + + // Allocation identifier is 0 as the MallocAllocator discards it + ComCcsdsUart::comQueue.configure(configurationTableUart, 0, ComCcsds::Allocation::memAllocator); + """ + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsUart::comQueue.cleanup(); + """ + } + + instance aggregator: Svc.ComAggregator base id ComCcsdsConfig.BASE_ID_UART + 0x06000 \ + queue size ComCcsdsConfig.QueueSizes.aggregator \ + stack size ComCcsdsConfig.StackSizes.aggregator + + # ---------------------------------------------------------------------- + # Passive Components + # ---------------------------------------------------------------------- + instance frameAccumulator: Svc.FrameAccumulator base id ComCcsdsConfig.BASE_ID_UART + 0x01000 \ + { + + phase Fpp.ToCpp.Phases.configObjects """ + Svc::FrameDetectors::CcsdsTcFrameDetector frameDetector; + """ + phase Fpp.ToCpp.Phases.configComponents """ + ComCcsdsUart::frameAccumulator.configure( + ConfigObjects::ComCcsdsUart_frameAccumulator::frameDetector, + 1, + ComCcsds::Allocation::memAllocator, + ComCcsdsConfig::BuffMgr::frameAccumulatorSize + ); + """ + + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsUart::frameAccumulator.cleanup(); + """ + } + + instance commsBufferManager: Svc.BufferManager base id ComCcsdsConfig.BASE_ID_UART + 0x02000 \ + { + phase Fpp.ToCpp.Phases.configObjects """ + Svc::BufferManager::BufferBins bins; + """ + + phase Fpp.ToCpp.Phases.configComponents """ + memset(&ConfigObjects::ComCcsdsUart_commsBufferManager::bins, 0, sizeof(ConfigObjects::ComCcsdsUart_commsBufferManager::bins)); + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[0].bufferSize = ComCcsdsConfig::BuffMgr::commsBuffSize; + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[0].numBuffers = ComCcsdsConfig::BuffMgr::commsBuffCount; + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[1].bufferSize = ComCcsdsConfig::BuffMgr::commsFileBuffSize; + ConfigObjects::ComCcsdsUart_commsBufferManager::bins.bins[1].numBuffers = ComCcsdsConfig::BuffMgr::commsFileBuffCount; + ComCcsdsUart::commsBufferManager.setup( + ComCcsdsConfig::BuffMgr::commsBuffMgrId, + 0, + ComCcsds::Allocation::memAllocator, + ConfigObjects::ComCcsdsUart_commsBufferManager::bins + ); + """ + + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsUart::commsBufferManager.cleanup(); + """ + } + + instance fprimeRouter: Svc.FprimeRouter base id ComCcsdsConfig.BASE_ID_UART + 0x03000 + + instance tcDeframer: Svc.Ccsds.TcDeframer base id ComCcsdsConfig.BASE_ID_UART + 0x04000 + + instance spacePacketDeframer: Svc.Ccsds.SpacePacketDeframer base id ComCcsdsConfig.BASE_ID_UART + 0x05000 + + # NOTE: name 'framer' is used for the framer that connects to the Com Adapter Interface for better subtopology interoperability + instance framer: Svc.Ccsds.TmFramer base id ComCcsdsConfig.BASE_ID_UART + 0x07000 + + instance spacePacketFramer: Svc.Ccsds.SpacePacketFramer base id ComCcsdsConfig.BASE_ID_UART + 0x08000 + + instance apidManager: Svc.Ccsds.ApidManager base id ComCcsdsConfig.BASE_ID_UART + 0x09000 + + instance comStub: Svc.ComStub base id ComCcsdsConfig.BASE_ID_UART + 0x0A000 + + topology FramingSubtopology { + # Usage Note: + # + # When importing this subtopology, users shall establish 5 port connections with a component implementing + # the Svc.Com (Svc/Interfaces/Com.fpp) interface. They are as follows: + # + # 1) Outputs: + # - ComCcsds.framer.dataOut -> [Svc.Com].dataIn + # - ComCcsds.frameAccumulator.dataReturnOut -> [Svc.Com].dataReturnIn + # 2) Inputs: + # - [Svc.Com].dataReturnOut -> ComCcsds.framer.dataReturnIn + # - [Svc.Com].comStatusOut -> ComCcsds.framer.comStatusIn + # - [Svc.Com].dataOut -> ComCcsds.frameAccumulator.dataIn + + + # Active Components + instance comQueue + + # Passive Components + instance commsBufferManager + instance frameAccumulator + instance fprimeRouter + instance tcDeframer + instance spacePacketDeframer + instance framer + instance spacePacketFramer + instance apidManager + instance aggregator + + connections Downlink { + # ComQueue <-> SpacePacketFramer + comQueue.dataOut -> spacePacketFramer.dataIn + spacePacketFramer.dataReturnOut -> comQueue.dataReturnIn + # SpacePacketFramer buffer and APID management + spacePacketFramer.bufferAllocate -> commsBufferManager.bufferGetCallee + spacePacketFramer.bufferDeallocate -> commsBufferManager.bufferSendIn + spacePacketFramer.getApidSeqCount -> apidManager.getApidSeqCountIn + # SpacePacketFramer <-> TmFramer + spacePacketFramer.dataOut -> aggregator.dataIn + aggregator.dataOut -> framer.dataIn + + framer.dataReturnOut -> aggregator.dataReturnIn + aggregator.dataReturnOut -> spacePacketFramer.dataReturnIn + + # ComStatus + framer.comStatusOut -> aggregator.comStatusIn + aggregator.comStatusOut -> spacePacketFramer.comStatusIn + spacePacketFramer.comStatusOut -> comQueue.comStatusIn + # (Outgoing) Framer <-> ComInterface connections shall be established by the user + } + + connections Uplink { + # (Incoming) ComInterface <-> FrameAccumulator connections shall be established by the user + # FrameAccumulator buffer allocations + frameAccumulator.bufferDeallocate -> commsBufferManager.bufferSendIn + frameAccumulator.bufferAllocate -> commsBufferManager.bufferGetCallee + # FrameAccumulator <-> TcDeframer + frameAccumulator.dataOut -> tcDeframer.dataIn + tcDeframer.dataReturnOut -> frameAccumulator.dataReturnIn + # TcDeframer <-> SpacePacketDeframer + tcDeframer.dataOut -> spacePacketDeframer.dataIn + spacePacketDeframer.dataReturnOut -> tcDeframer.dataReturnIn + # SpacePacketDeframer APID validation + spacePacketDeframer.validateApidSeqCount -> apidManager.validateApidSeqCountIn + # SpacePacketDeframer <-> Router + spacePacketDeframer.dataOut -> fprimeRouter.dataIn + fprimeRouter.dataReturnOut -> spacePacketDeframer.dataReturnIn + # Router buffer allocations + fprimeRouter.bufferAllocate -> commsBufferManager.bufferGetCallee + fprimeRouter.bufferDeallocate -> commsBufferManager.bufferSendIn + } + } # end FramingSubtopology + + # This subtopology uses FramingSubtopology with a ComStub component for Com Interface + topology Subtopology { + import FramingSubtopology + + instance comStub + + connections ComStub { + # Framer <-> ComStub (Downlink) + framer.dataOut -> comStub.dataIn + comStub.dataReturnOut -> framer.dataReturnIn + comStub.comStatusOut -> framer.comStatusIn + + # ComStub <-> FrameAccumulator (Uplink) + comStub.dataOut -> frameAccumulator.dataIn + frameAccumulator.dataReturnOut -> comStub.dataReturnIn + } + } # end Subtopology + +} # end ComCcsds diff --git a/FprimeZephyrReference/ComCcsdsUart/PingEntries.hpp b/FprimeZephyrReference/ComCcsdsUart/PingEntries.hpp new file mode 100644 index 0000000..eeadc7d --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsUart/PingEntries.hpp @@ -0,0 +1,9 @@ + +#ifndef COMCCSDS_PINGENTRIES_HPP +#define COMCCSDS_PINGENTRIES_HPP + +namespace PingEntries { +// No ping-enabled components in ComCcsds subtopology +} + +#endif diff --git a/FprimeZephyrReference/ComCcsdsUart/SubtopologyTopologyDefs.hpp b/FprimeZephyrReference/ComCcsdsUart/SubtopologyTopologyDefs.hpp new file mode 100644 index 0000000..d130ae3 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsUart/SubtopologyTopologyDefs.hpp @@ -0,0 +1,20 @@ +#ifndef COMCCSDSSUBTOPOLOGY_DEFS_HPP +#define COMCCSDSSUBTOPOLOGY_DEFS_HPP + +#include +#include +#include +#include "ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp" +#include "Svc/Subtopologies/ComCcsds/ComCcsdsConfig/FppConstantsAc.hpp" + +namespace ComCcsds { +struct SubtopologyState { + // Empty - no external state needed for ComCcsds subtopology +}; + +struct TopologyState { + SubtopologyState comCcsds; +}; +} // namespace ComCcsds + +#endif diff --git a/FprimeZephyrReference/ComCcsdsUart/docs/sdd.md b/FprimeZephyrReference/ComCcsdsUart/docs/sdd.md new file mode 100644 index 0000000..9ebc9be --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsUart/docs/sdd.md @@ -0,0 +1,5 @@ +# ComCcsdsUart + +This is a clone with a renamed module of the F Prime's Svc::ComCcsds. + +See: [Svc::ComCcsds](../../../lib/fprime/Svc/Subtopologies/ComCcsds/docs/sdd.md) diff --git a/FprimeZephyrReference/Components/ComDelay/ComDelay.cpp b/FprimeZephyrReference/Components/ComDelay/ComDelay.cpp index 2d1c073..a94cdef 100644 --- a/FprimeZephyrReference/Components/ComDelay/ComDelay.cpp +++ b/FprimeZephyrReference/Components/ComDelay/ComDelay.cpp @@ -51,7 +51,6 @@ void ComDelay ::run_handler(FwIndexType portNum, U32 context) { bool valid = this->m_last_status_valid.compare_exchange_strong(expected, false); if (valid) { this->comStatusOut_out(0, this->m_last_status); - this->timeout_out(0, 0); } } diff --git a/FprimeZephyrReference/Components/ComDelay/ComDelay.fpp b/FprimeZephyrReference/Components/ComDelay/ComDelay.fpp index 6c14f0c..a0122ff 100644 --- a/FprimeZephyrReference/Components/ComDelay/ComDelay.fpp +++ b/FprimeZephyrReference/Components/ComDelay/ComDelay.fpp @@ -5,9 +5,6 @@ module Components { @ Rate schedule port used to trigger radio transmission sync input port run: Svc.Sched - @ Rate schedule port used to trigger aggregation timeout - output port timeout: Svc.Sched - @ Input comStatus from radio component sync input port comStatusIn: Fw.SuccessCondition diff --git a/FprimeZephyrReference/ReferenceDeployment/Main.cpp b/FprimeZephyrReference/ReferenceDeployment/Main.cpp index e3148ff..1747956 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Main.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Main.cpp @@ -11,6 +11,7 @@ #include const struct device* serial = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart0)); +const struct device* lora = DEVICE_DT_GET(DT_NODELABEL(lora0)); int main(int argc, char* argv[]) { // ** DO NOT REMOVE **// @@ -22,6 +23,7 @@ int main(int argc, char* argv[]) { Os::init(); // Object for communicating state to the topology ReferenceDeployment::TopologyState inputs; + inputs.loraDevice = lora; inputs.uartDevice = serial; inputs.baudRate = 115200; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 4a0e736..4c2e3fc 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -2,8 +2,11 @@ telemetry packets ReferenceDeploymentPackets { packet Health id 1 group 1 { CdhCore.cmdDisp.CommandsDispatched + CdhCore.cmdDisp.CommandsDropped ComCcsds.comQueue.comQueueDepth ComCcsds.commsBufferManager.HiBuffs + ComCcsdsUart.comQueue.comQueueDepth + ComCcsdsUart.commsBufferManager.HiBuffs ReferenceDeployment.rateGroup10Hz.RgMaxTime ReferenceDeployment.rateGroup1Hz.RgMaxTime } @@ -12,6 +15,8 @@ telemetry packets ReferenceDeploymentPackets { CdhCore.$health.PingLateWarnings ComCcsds.commsBufferManager.NoBuffs ComCcsds.commsBufferManager.EmptyBuffs + ComCcsdsUart.commsBufferManager.NoBuffs + ComCcsdsUart.commsBufferManager.EmptyBuffs ReferenceDeployment.rateGroup10Hz.RgCycleSlips ReferenceDeployment.rateGroup1Hz.RgCycleSlips } @@ -21,6 +26,9 @@ telemetry packets ReferenceDeploymentPackets { ComCcsds.commsBufferManager.TotalBuffs ComCcsds.commsBufferManager.CurrBuffs ComCcsds.comQueue.buffQueueDepth + ComCcsdsUart.commsBufferManager.TotalBuffs + ComCcsdsUart.commsBufferManager.CurrBuffs + ComCcsdsUart.comQueue.buffQueueDepth CdhCore.tlmSend.SendLevel } @@ -41,6 +49,10 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.lis2mdlManager.MagneticField } + packet LoRa id 7 group 4 { + lora.LastRssi + lora.LastSnr + } } omit { diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index d06583f..2130696 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -81,7 +81,9 @@ void setupTopology(const TopologyState& state) { // Autocoded task kick-off (active components). Function provided by autocoder. startTasks(state); - // Uplink is configured for receive so a socket task is started + // We have a pipeline for both the LoRa and UART drive to allow for ground harness debugging an + // for over-the-air communications. + lora.start(state.loraDevice, Zephyr::TransmitState::DISABLED); comDriver.configure(state.uartDevice, state.baudRate); } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp index 3912dbb..81064f6 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -70,6 +70,7 @@ namespace ReferenceDeployment { */ struct TopologyState { const device* uartDevice; //!< UART device path for communication + const device* loraDevice; //!< LoRa device path for communication U32 baudRate; //!< Baud rate for UART communication CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 64925bc..bdbb36a 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -75,4 +75,10 @@ module ReferenceDeployment { instance prmDb: Components.NullPrmDb base id 0x10024000 instance comDelay: Components.ComDelay base id 0x10025000 + + instance lora: Zephyr.LoRa base id 0x10026000 + + instance comSplitterEvents: Svc.ComSplitter base id 0x10027000 + + instance comSplitterTelemetry: Svc.ComSplitter base id 0x10028000 } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index e67efb4..89626e3 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -15,7 +15,8 @@ module ReferenceDeployment { # Subtopology imports # ---------------------------------------------------------------------- import CdhCore.Subtopology - import ComCcsds.Subtopology + import ComCcsds.FramingSubtopology + import ComCcsdsUart.Subtopology # ---------------------------------------------------------------------- # Instances used in the topology @@ -24,7 +25,7 @@ module ReferenceDeployment { instance rateGroup1Hz instance rateGroupDriver instance timer - instance comDriver + instance lora instance gpioDriver instance gpioBurnwire0 instance gpioBurnwire1 @@ -37,6 +38,10 @@ module ReferenceDeployment { instance bootloaderTrigger instance comDelay instance burnwire + instance comSplitterEvents + instance comSplitterTelemetry + # For UART sideband communication + instance comDriver # ---------------------------------------------------------------------- # Pattern graph specifiers @@ -62,27 +67,49 @@ module ReferenceDeployment { connections ComCcsds_CdhCore { # Core events and telemetry to communication queue - CdhCore.events.PktSend -> ComCcsds.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.EVENTS] - CdhCore.tlmSend.PktSend -> ComCcsds.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.TELEMETRY] + CdhCore.events.PktSend -> comSplitterEvents.comIn + comSplitterEvents.comOut-> ComCcsds.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.EVENTS] + comSplitterEvents.comOut-> ComCcsdsUart.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.EVENTS] + + CdhCore.tlmSend.PktSend -> comSplitterTelemetry.comIn + comSplitterTelemetry.comOut -> ComCcsds.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.TELEMETRY] + comSplitterTelemetry.comOut -> ComCcsdsUart.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.TELEMETRY] # Router to Command Dispatcher ComCcsds.fprimeRouter.commandOut -> CdhCore.cmdDisp.seqCmdBuff CdhCore.cmdDisp.seqCmdStatus -> ComCcsds.fprimeRouter.cmdResponseIn + ComCcsdsUart.fprimeRouter.commandOut -> CdhCore.cmdDisp.seqCmdBuff + CdhCore.cmdDisp.seqCmdStatus -> ComCcsdsUart.fprimeRouter.cmdResponseIn + } + + connections CommunicationsRadio { + lora.allocate -> ComCcsds.commsBufferManager.bufferGetCallee + lora.deallocate -> ComCcsds.commsBufferManager.bufferSendIn + + # ComDriver <-> ComStub (Uplink) + lora.dataOut -> ComCcsds.frameAccumulator.dataIn + ComCcsds.frameAccumulator.dataReturnOut -> lora.dataReturnIn + + # ComStub <-> ComDriver (Downlink) + ComCcsds.framer.dataOut -> lora.dataIn + lora.dataReturnOut -> ComCcsds.framer.dataReturnIn + lora.comStatusOut -> comDelay.comStatusIn + comDelay.comStatusOut ->ComCcsds.framer.comStatusIn } - connections Communications { + connections CommunicationsUart { # ComDriver buffer allocations - comDriver.allocate -> ComCcsds.commsBufferManager.bufferGetCallee - comDriver.deallocate -> ComCcsds.commsBufferManager.bufferSendIn + comDriver.allocate -> ComCcsdsUart.commsBufferManager.bufferGetCallee + comDriver.deallocate -> ComCcsdsUart.commsBufferManager.bufferSendIn # ComDriver <-> ComStub (Uplink) - comDriver.$recv -> ComCcsds.comStub.drvReceiveIn - ComCcsds.comStub.drvReceiveReturnOut -> comDriver.recvReturnIn + comDriver.$recv -> ComCcsdsUart.comStub.drvReceiveIn + ComCcsdsUart.comStub.drvReceiveReturnOut -> comDriver.recvReturnIn # ComStub <-> ComDriver (Downlink) - ComCcsds.comStub.drvSendOut -> comDriver.$send - comDriver.ready -> ComCcsds.comStub.drvConnected + ComCcsdsUart.comStub.drvSendOut -> comDriver.$send + comDriver.ready -> ComCcsdsUart.comStub.drvConnected } connections RateGroups { @@ -92,6 +119,8 @@ module ReferenceDeployment { # High rate (10Hz) rate group rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup10Hz] -> rateGroup10Hz.CycleIn rateGroup10Hz.RateGroupMemberOut[0] -> comDriver.schedIn + rateGroup10Hz.RateGroupMemberOut[1] -> ComCcsdsUart.aggregator.timeout + rateGroup10Hz.RateGroupMemberOut[2] -> ComCcsds.aggregator.timeout # Slow rate (1Hz) rate group rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1Hz] -> rateGroup1Hz.CycleIn diff --git a/FprimeZephyrReference/project/config/CMakeLists.txt b/FprimeZephyrReference/project/config/CMakeLists.txt index 15360ff..cf115d7 100644 --- a/FprimeZephyrReference/project/config/CMakeLists.txt +++ b/FprimeZephyrReference/project/config/CMakeLists.txt @@ -9,7 +9,9 @@ register_fprime_config( "${CMAKE_CURRENT_LIST_DIR}/CdhCoreTlmConfig.fpp" "${CMAKE_CURRENT_LIST_DIR}/CdhCoreFatalHandlerConfig.fpp" "${CMAKE_CURRENT_LIST_DIR}/ComCcsdsConfig.fpp" + "${CMAKE_CURRENT_LIST_DIR}/ComCfg.fpp" "${CMAKE_CURRENT_LIST_DIR}/CommandDispatcherImplCfg.hpp" + "${CMAKE_CURRENT_LIST_DIR}/LoRaCfg.hpp" "${CMAKE_CURRENT_LIST_DIR}/FpConfig.h" "${CMAKE_CURRENT_LIST_DIR}/TlmPacketizerCfg.hpp" INTERFACE diff --git a/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp b/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp index 4f8bc6d..c39d377 100644 --- a/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp +++ b/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp @@ -1,23 +1,27 @@ module ComCcsdsConfig { #Base ID for the ComCcsds Subtopology, all components are offsets from this base ID constant BASE_ID = 0x02000000 + constant BASE_ID_UART = 0x21000000 module QueueSizes { - constant comQueue = 10 + constant comQueue = 5 + constant aggregator = 15 } module StackSizes { constant comQueue = 8 * 1024 # Must match prj.conf thread stack size + constant aggregator = 8 * 1024 # Must match prj.conf thread stack size } module Priorities { - constant comQueue = 5 + constant aggregator = 5 # Aggregator (consumer) must have higher priority than comQueue (producer) + constant comQueue = 6 # ComQueue has higher priority than data producers (e.g. events, telemetry) } # Queue configuration constants module QueueDepths { - constant events = 20 - constant tlm = 20 + constant events = 10 + constant tlm = 5 constant file = 1 } @@ -29,8 +33,8 @@ module ComCcsdsConfig { # Buffer management constants module BuffMgr { - constant frameAccumulatorSize = 2048 - constant commsBuffSize = 2048 + constant frameAccumulatorSize = 1024 # Must be at least as large as the comm buffer size + constant commsBuffSize = 1024 # Size of ring buffer constant commsFileBuffSize = 1 constant commsBuffCount = 5 constant commsFileBuffCount = 1 diff --git a/FprimeZephyrReference/project/config/ComCfg.fpp b/FprimeZephyrReference/project/config/ComCfg.fpp new file mode 100644 index 0000000..af6ac4b --- /dev/null +++ b/FprimeZephyrReference/project/config/ComCfg.fpp @@ -0,0 +1,52 @@ +# ====================================================================== +# FPP file for configuration of the communications stack +# +# The only reason to modify these definitions is if you are writing your own +# Framer/Deframer implementations and need more contextual data than what is +# defined +# ====================================================================== + +@ The width of packet descriptors when they are serialized by the framework +type FwPacketDescriptorType = U16 + +module ComCfg { + + # Needed in dictionary: + # - spacecraftId + # - TmFrameFixedSize + # - potentially APID enum ? + constant SpacecraftId = 0x0044 # Spacecraft ID (10 bits) + constant TmFrameFixedSize = 248 # Needs to be at least COM_BUFFER_MAX_SIZE + (2 * SpacePacketHeaderSize) + 1 + constant AggregationSize = TmFrameFixedSize - 6 - 6 - 1 - 2 # 2 header (6) + 1 idle byte + 2 trailer bytes + + @ APIDs are 11 bits in the Space Packet protocol, so we use U16. Max value 7FF + enum Apid : FwPacketDescriptorType { + # APIDs prefixed with FW are reserved for F Prime and need to be present + # in the enumeration. Their values can be changed + FW_PACKET_COMMAND = 0x0000 @< Command packet type - incoming + FW_PACKET_TELEM = 0x0001 @< Telemetry packet type - outgoing + FW_PACKET_LOG = 0x0002 @< Log type - outgoing + FW_PACKET_FILE = 0x0003 @< File type - incoming and outgoing + FW_PACKET_PACKETIZED_TLM = 0x0004 @< Packetized telemetry packet type + FW_PACKET_DP = 0x0005 @< Data Product packet type + FW_PACKET_IDLE = 0x0006 @< F Prime idle + FW_PACKET_HAND = 0x00FE @< F Prime handshake + FW_PACKET_UNKNOWN = 0x00FF @< F Prime unknown packet + SPP_IDLE_PACKET = 0x07FF @< Per Space Packet Standard, all 1s (11bits) is reserved for Idle Packets + INVALID_UNINITIALIZED = 0x0800 @< Anything equal or higher value is invalid and should not be used + } default INVALID_UNINITIALIZED + + @ Type used to pass context info between components during framing/deframing + struct FrameContext { + comQueueIndex: FwIndexType @< Queue Index used by the ComQueue, other components shall not modify + apid: Apid @< 11 bits APID in CCSDS + sequenceCount: U16 @< 14 bit Sequence count - sequence count is incremented per APID + vcId: U8 @< 6 bit Virtual Channel ID - used for TC and TM + } default { + comQueueIndex = 0 + apid = Apid.FW_PACKET_UNKNOWN + sequenceCount = 0 + vcId = 1 + } + +} diff --git a/FprimeZephyrReference/project/config/FpConfig.h b/FprimeZephyrReference/project/config/FpConfig.h index 3adfa68..bf1fe7b 100644 --- a/FprimeZephyrReference/project/config/FpConfig.h +++ b/FprimeZephyrReference/project/config/FpConfig.h @@ -166,7 +166,7 @@ extern "C" { // Specifies the size of the buffer that contains a communications packet. #ifndef FW_COM_BUFFER_MAX_SIZE -#define FW_COM_BUFFER_MAX_SIZE 512 +#define FW_COM_BUFFER_MAX_SIZE 233 #endif // Specifies the size of the buffer attached to state machine signals. @@ -327,6 +327,10 @@ extern "C" { #define FW_FILE_CHUNK_SIZE 512 //!< Chunk size for working with files in the OSAL layer #endif +#ifndef FW_ASSERT_COUNT_MAX +#define FW_ASSERT_COUNT_MAX 4 +#endif + // *** NOTE configuration checks are in Fw/Cfg/ConfigCheck.cpp in order to have // the type definitions in Fw/Types/BasicTypes available. #ifdef __cplusplus diff --git a/FprimeZephyrReference/project/config/LoRaCfg.hpp b/FprimeZephyrReference/project/config/LoRaCfg.hpp new file mode 100644 index 0000000..0de46af --- /dev/null +++ b/FprimeZephyrReference/project/config/LoRaCfg.hpp @@ -0,0 +1,12 @@ +#ifndef LORA_CFG_HPP +#define LORA_CFG_HPP +#include +#include +namespace LoRaConfig { +const U32 FREQUENCY = 437400000; //!< LoRa frequency in Hz +lora_signal_bandwidth BANDWIDTH = BW_125_KHZ; //!< LoRa bandwidth +const I8 TX_POWER = 14; //!< LoRa transmission power in dBm +const U16 PREAMBLE_LENGTH = 8; //!< LoRa preamble length +U8 HEADER[] = {0, 0, 0, 0}; //!< LoRa header (not used) +} // namespace LoRaConfig +#endif // LORA_CFG_HPP diff --git a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 96f2024..3b99c34 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 = 6; +static const FwChanIdType MAX_PACKETIZER_PACKETS = 7; 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 diff --git a/Makefile b/Makefile index c27a53e..c6e02bc 100644 --- a/Makefile +++ b/Makefile @@ -17,8 +17,17 @@ fprime-venv: uv ## Create a virtual environment @$(UV) venv fprime-venv --allow-existing @$(UV) pip install --prerelease=allow --requirement requirements.txt -patch-gps-package: - cp custom_space_data_link.py fprime-venv/lib/python3.13/site-packages/fprime_gds/common/communication/ccsds/space_data_link.py +.PHONY: zephyr-setup +zephyr-setup: fprime-venv ## Set up Zephyr environment + @test -d lib/zephyr-workspace/modules/hal/rpi_pico || test -d ../lib/zephyr-workspace/modules/hal/rpi_pico || { \ + echo "Setting up Zephyr environment..."; \ + rm -rf ../.west/ && \ + $(UVX) west init --local . && \ + $(UVX) west update && \ + $(UVX) west zephyr-export && \ + $(UV) run west packages pip --install && \ + $(UV) run west sdk install --toolchains arm-zephyr-eabi; \ + } ##@ Development diff --git a/README.md b/README.md index ef4019b..da45089 100644 --- a/README.md +++ b/README.md @@ -72,3 +72,32 @@ Then, in another terminal, run the following command to execute the integration ```sh make test-integration ``` + +## Running The Radio With CircuitPython + +To test the radio setup easily, you can use CircuitPython code on one board and fprime-zephyr on another. This provides a simple client/server setup and lets you observe what data is being sent through the radio. + + +On the board you want to receive data, make sure you have CircuitPython installed. Follow [these instructions](https://proveskit.github.io/pysquared/getting-started/). You can install the flight software or ground station code from that tutorial as these have the libraries you need, or simply install the CircuitPython firmware and manually add the required libraries. + + +Once you have CircuitPython running, upload the files from the ```circuit-python-lora-passthrough``` folder in this repo. Make sure to overwrite any existing ```boot.py``` and ```code.py``` files on your CircuitPython board with the ones from this folder. + +```boot.py``` enables both virtual serial ports that the device presents over USB. This allows you to use one for the console and one for data. ```code.py``` acts as a LoRa radio forwarder over USB serial: The console port is used for logging and debugging, and is the first serial port that appears when the board is connected. The data port is used for actual data transfer, and is the second serial port. + +1. Open the console port on your computer. This is the first serial port that opens when you plug in the circuitpython board. It should start by printing: + +``` +[INFO] LoRa Receiver receiving packets +[INFO] Packets received: 0 +``` + +Once you have the board running the proves-core-reference radio code (make sure its plugged in!), you should start receiving packets and seeing this on the serial port + +2. + +Now you want to be able to send commands through the radio. To do this, connect the gds to the circuitpython data port. Run the fprime-gds with the --uart-device parameter set to the serial port that is the second serial port that shows up when you plug in your circuitpython board + +Depending on the comdelay, the gds should turn green every time a packet is sent. If you want to change this parameter use + +```ReferenceDeployment.comDelay.DIVIDER_PRM_SET``` on the gds. You can set it down to 2, but setting it to 1 may cause issues. diff --git a/custom_space_data_link.py b/custom_space_data_link.py deleted file mode 100644 index 0439afd..0000000 --- a/custom_space_data_link.py +++ /dev/null @@ -1,198 +0,0 @@ -"""F Prime Framer/Deframer Implementation of the CCSDS Space Data Link (TC/TM) Protocols""" - -import copy -import struct -import sys - -import crc -from fprime_gds.common.communication.framing import FramerDeframer -from fprime_gds.plugin.definitions import gds_plugin_implementation - - -class SpaceDataLinkFramerDeframer(FramerDeframer): - """CCSDS Framer/Deframer Implementation for the TC (uplink / framing) and TM (downlink / deframing) - protocols. This FramerDeframer is used for framing TC data for uplink and deframing TM data for downlink. - """ - - SEQUENCE_NUMBER_MAXIMUM = 256 - TC_HEADER_SIZE = 5 - TM_HEADER_SIZE = 6 - TM_FIXED_FRAME_SIZE = 248 - TM_TRAILER_SIZE = 2 - TC_TRAILER_SIZE = 2 - - # As per CCSDS standard, use CRC-16 CCITT config with init value - # all 1s and final XOR value of 0x0000 - CRC_CCITT_CONFIG = crc.Configuration( - width=16, - polynomial=0x1021, - init_value=0xFFFF, - final_xor_value=0x0000, - ) - CRC_CALCULATOR = crc.Calculator(CRC_CCITT_CONFIG) - - def __init__(self, scid, vcid): - """ """ - self.scid = scid - self.vcid = vcid - self.sequence_number = 0 - - def frame(self, data): - """Frame the supplied data in a TC frame""" - space_packet_bytes = data - # CCSDS TC protocol defines the length token as number of bytes in full frame, minus 1 - # so we add to packet size the size of the header and trailer and subtract 1 - length = ( - len(space_packet_bytes) + self.TC_HEADER_SIZE + self.TC_TRAILER_SIZE - 1 - ) - assert length < (pow(2, 10) - 1), "Length too-large for CCSDS format" - - # CCSDS TC Header: - # 2b - 00 - TF version number - # 1b - 0/1 - 0 enable FARM checks, 1 bypass FARM - # 1b - 0/1 - 0 = data (Type-D), 1 = control information (Type-C) - # 2b - 00 - Reserved - # 10b - XX - Spacecraft id - # 6b - XX - Virtual Channel ID - # 10b - XX - Frame length - # 8b - XX - Frame sequence number - - # First 16 bits: - header_val1_u16 = ( - (0 << 14) # TF version number (2 bits) - | (1 << 13) # Bypass FARM (1 bit) - | (0 << 12) # Type-D (1 bit) - | (0 << 10) # Reserved (2 bits) - | (self.scid & 0x3FF) # SCID (10 bits) - ) - # Second 16 bits: - header_val2_u16 = ( - ((self.vcid & 0x3F) << 10) # VCID (6 bits) - | (length & 0x3FF) # Frame length (10 bits) - ) - # 8 bit sequence number - always 0 in bypass FARM mode - header_val3_u8 = 0 - header_bytes = struct.pack( - ">HHB", header_val1_u16, header_val2_u16, header_val3_u8 - ) - full_bytes_no_crc = header_bytes + space_packet_bytes - assert len(header_bytes) == self.TC_HEADER_SIZE, ( - "CCSDS primary header must be 5 octets long" - ) - assert len(full_bytes_no_crc) == self.TC_HEADER_SIZE + len(data), ( - "Malformed packet generated" - ) - - full_bytes = full_bytes_no_crc + struct.pack( - ">H", self.CRC_CALCULATOR.checksum(full_bytes_no_crc) - ) - return full_bytes - - def get_sequence_number(self): - """Get the sequence number and increment - used for TM deframing - - This function will return the current sequence number and then increment the sequence number for the next round. - - Return: - current sequence number - """ - sequence = self.sequence_number - self.sequence_number = (self.sequence_number + 1) % self.SEQUENCE_NUMBER_MAXIMUM - return sequence - - def deframe(self, data, no_copy=False): - """Deframe TM frames""" - discarded = b"" - if not no_copy: - data = copy.copy(data) - # Continue until there is not enough data for the header, or until a packet is found (return) - while len(data) >= self.TM_FIXED_FRAME_SIZE: - # Read header information - sc_and_channel_ids = struct.unpack_from(">H", data) - spacecraft_id = (sc_and_channel_ids[0] & 0x3FF0) >> 4 - virtual_channel_id = (sc_and_channel_ids[0] & 0x000E) >> 1 - # Check if the header is correct with regards to expected spacecraft and VC IDs - if spacecraft_id != self.scid or virtual_channel_id != self.vcid: - # If the header is invalid, rotate away a Byte and keep processing - discarded += data[0:1] - data = data[1:] - continue - # Spacecraft ID and Virtual Channel ID match, so we look at end of frame for CRC - crc_offset = self.TM_FIXED_FRAME_SIZE - self.TM_TRAILER_SIZE - transmitted_crc = struct.unpack_from(">H", data, crc_offset)[0] - if transmitted_crc == self.CRC_CALCULATOR.checksum(data[:crc_offset]): - # CRC is valid, so we return the deframed data - deframed_data_len = ( - self.TM_FIXED_FRAME_SIZE - - self.TM_TRAILER_SIZE - - self.TM_HEADER_SIZE - ) - deframed = struct.unpack_from( - f">{deframed_data_len}s", data, self.TM_HEADER_SIZE - )[0] - # Consume the fixed size frame - data = data[self.TM_FIXED_FRAME_SIZE :] - return deframed, data, discarded - - print( - "[WARNING] Checksum validation failed.", - file=sys.stderr, - ) - # Bad checksum, rotate 1 and keep looking for non-garbage - discarded += data[0:1] - data = data[1:] - continue - return None, data, discarded - - @classmethod - def get_arguments(cls): - """Arguments to request from the CLI""" - return { - ("--scid",): { - "type": lambda input_arg: int(input_arg, 0), - "help": "Spacecraft ID", - "default": 0x44, - "required": False, - }, - ("--vcid",): { - "type": lambda input_arg: int(input_arg, 0), - "help": "Virtual channel ID", - "default": 1, - "required": False, - }, - } - - @classmethod - def check_arguments(cls, scid, vcid): - """Check arguments from the CLI - - Confirms that the input arguments are valid for this framer/deframer. - - Args: - scid: spacecraft id - vcid: virtual channel id - """ - if scid is None: - raise TypeError("Spacecraft ID not specified") - if scid < 0: - raise TypeError(f"Spacecraft ID {scid} is negative") - if scid > 0x3FF: - raise TypeError(f"Spacecraft ID {scid} is larger than {0x3FF}") - - if vcid is None: - raise TypeError("Virtual Channel ID not specified") - if vcid < 0: - raise TypeError(f"Virtual Channel ID {vcid} is negative") - if vcid > 0x3F: - raise TypeError(f"Virtual Channel ID {vcid} is larger than {0x3FF}") - - @classmethod - def get_name(cls): - """Name of this implementation provided to CLI""" - return "raw-space-data-link" - - @classmethod - @gds_plugin_implementation - def register_framing_plugin(cls): - """Register the MyPlugin plugin""" - return cls diff --git a/fprime-gds.yml b/fprime-gds.yml index 7145707..c4ef300 100644 --- a/fprime-gds.yml +++ b/fprime-gds.yml @@ -4,3 +4,4 @@ command-line-options: no-app: dictionary: build-artifacts/zephyr/fprime-zephyr-deployment/dict/ReferenceDeploymentTopologyDictionary.json output-unframed-data: "-" + frame-size: 248 diff --git a/lib/fprime b/lib/fprime index abb09e4..626473f 160000 --- a/lib/fprime +++ b/lib/fprime @@ -1 +1 @@ -Subproject commit abb09e4a681cd586ac719a7517430637cdc46199 +Subproject commit 626473fc4e89e491d43ad95bb0d1b7727014c80d diff --git a/lib/fprime-zephyr b/lib/fprime-zephyr index 4a149bb..dc83bef 160000 --- a/lib/fprime-zephyr +++ b/lib/fprime-zephyr @@ -1 +1 @@ -Subproject commit 4a149bbed5c7c86fbac44ab8a65791eff97370f9 +Subproject commit dc83bef61c25c65d3854cf65b045daf42a6a58f9 diff --git a/prj.conf b/prj.conf index f171594..72c56e2 100644 --- a/prj.conf +++ b/prj.conf @@ -36,7 +36,7 @@ CONFIG_DYNAMIC_THREAD=y CONFIG_KERNEL_MEM_POOL=y CONFIG_DYNAMIC_THREAD_ALLOC=n CONFIG_DYNAMIC_THREAD_PREFER_POOL=y -CONFIG_DYNAMIC_THREAD_POOL_SIZE=10 +CONFIG_DYNAMIC_THREAD_POOL_SIZE=15 # Num threads in the thread pool CONFIG_DYNAMIC_THREAD_STACK_SIZE=8192 # Size of thread stack in thread pool, must be >= Thread Pool size in F' diff --git a/settings.ini b/settings.ini index d27e929..dde51b9 100644 --- a/settings.ini +++ b/settings.ini @@ -7,4 +7,4 @@ default_toolchain: zephyr default_cmake_options: FPRIME_ENABLE_FRAMEWORK_UTS=OFF FPRIME_ENABLE_AUTOCODER_UTS=OFF BOARD_ROOT=. - BOARD=proves_flight_control_board_v5c/rp2350a/m33 + BOARD=proves_flight_control_board_v5d/rp2350a/m33