diff --git a/.gitmodules b/.gitmodules index ef179827..50f7b4d9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "lib/fprime-zephyr"] path = lib/fprime-zephyr url = https://github.com/fprime-community/fprime-zephyr.git +[submodule "lib/RadioLib"] + path = lib/RadioLib + url = https://github.com/jgromes/RadioLib diff --git a/FprimeZephyrReference/CMakeLists.txt b/FprimeZephyrReference/CMakeLists.txt index 95ee3b94..b9d1d0f2 100644 --- a/FprimeZephyrReference/CMakeLists.txt +++ b/FprimeZephyrReference/CMakeLists.txt @@ -5,4 +5,5 @@ 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}/ComCcsdsSband/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ReferenceDeployment/") diff --git a/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt b/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt new file mode 100644 index 00000000..686067e9 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/CMakeLists.txt @@ -0,0 +1,12 @@ + +register_fprime_module( + EXCLUDE_FROM_ALL + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/ComCcsdsSband.fpp" + HEADERS + "${CMAKE_CURRENT_LIST_DIR}/SubtopologyTopologyDefs.hpp" + "${CMAKE_CURRENT_LIST_DIR}/PingEntries.hpp" + DEPENDS + Svc_Subtopologies_ComCcsds_ComCcsdsConfig + INTERFACE +) diff --git a/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp b/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp new file mode 100644 index 00000000..6c1d8704 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/ComCcsdsSband.fpp @@ -0,0 +1,203 @@ +module ComCcsdsSband { + + # 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_SBAND + 0x00000 \ + queue size ComCcsdsConfig.QueueSizes.comQueue \ + stack size ComCcsdsConfig.StackSizes.comQueue \ + priority ComCcsdsConfig.Priorities.comQueue \ + { + phase Fpp.ToCpp.Phases.configComponents """ + using namespace ComCcsdsSband; + Svc::ComQueue::QueueConfigurationTable configurationTableSband; + + // Events (highest-priority) + configurationTableSband.entries[Ports_ComPacketQueue::EVENTS].depth = ComCcsdsConfig::QueueDepths::events; + configurationTableSband.entries[Ports_ComPacketQueue::EVENTS].priority = ComCcsdsConfig::QueuePriorities::events; + + // Telemetry + configurationTableSband.entries[Ports_ComPacketQueue::TELEMETRY].depth = ComCcsdsConfig::QueueDepths::tlm; + configurationTableSband.entries[Ports_ComPacketQueue::TELEMETRY].priority = ComCcsdsConfig::QueuePriorities::tlm; + + // File Downlink Queue (buffer queue using NUM_CONSTANTS offset) + configurationTableSband.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].depth = ComCcsdsConfig::QueueDepths::file; + configurationTableSband.entries[Ports_ComPacketQueue::NUM_CONSTANTS + Ports_ComBufferQueue::FILE].priority = ComCcsdsConfig::QueuePriorities::file; + + // Allocation identifier is 0 as the MallocAllocator discards it + ComCcsdsSband::comQueue.configure(configurationTableSband, 0, ComCcsds::Allocation::memAllocator); + """ + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsSband::comQueue.cleanup(); + """ + } + + instance aggregator: Svc.ComAggregator base id ComCcsdsConfig.BASE_ID_SBAND + 0x06000 \ + queue size ComCcsdsConfig.QueueSizes.aggregator \ + stack size ComCcsdsConfig.StackSizes.aggregator + + # ---------------------------------------------------------------------- + # Passive Components + # ---------------------------------------------------------------------- + instance frameAccumulator: Svc.FrameAccumulator base id ComCcsdsConfig.BASE_ID_SBAND + 0x01000 \ + { + + phase Fpp.ToCpp.Phases.configObjects """ + Svc::FrameDetectors::CcsdsTcFrameDetector frameDetector; + """ + phase Fpp.ToCpp.Phases.configComponents """ + ComCcsdsSband::frameAccumulator.configure( + ConfigObjects::ComCcsdsSband_frameAccumulator::frameDetector, + 1, + ComCcsds::Allocation::memAllocator, + ComCcsdsConfig::BuffMgr::frameAccumulatorSize + ); + """ + + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsSband::frameAccumulator.cleanup(); + """ + } + + instance commsBufferManager: Svc.BufferManager base id ComCcsdsConfig.BASE_ID_SBAND + 0x02000 \ + { + phase Fpp.ToCpp.Phases.configObjects """ + Svc::BufferManager::BufferBins bins; + """ + + phase Fpp.ToCpp.Phases.configComponents """ + memset(&ConfigObjects::ComCcsdsSband_commsBufferManager::bins, 0, sizeof(ConfigObjects::ComCcsdsSband_commsBufferManager::bins)); + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[0].bufferSize = ComCcsdsConfig::BuffMgr::commsBuffSize; + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[0].numBuffers = ComCcsdsConfig::BuffMgr::commsBuffCount; + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[1].bufferSize = ComCcsdsConfig::BuffMgr::commsFileBuffSize; + ConfigObjects::ComCcsdsSband_commsBufferManager::bins.bins[1].numBuffers = ComCcsdsConfig::BuffMgr::commsFileBuffCount; + ComCcsdsSband::commsBufferManager.setup( + ComCcsdsConfig::BuffMgr::commsBuffMgrId, + 0, + ComCcsds::Allocation::memAllocator, + ConfigObjects::ComCcsdsSband_commsBufferManager::bins + ); + """ + + phase Fpp.ToCpp.Phases.tearDownComponents """ + ComCcsdsSband::commsBufferManager.cleanup(); + """ + } + + instance fprimeRouter: Svc.FprimeRouter base id ComCcsdsConfig.BASE_ID_SBAND + 0x03000 + + instance tcDeframer: Svc.Ccsds.TcDeframer base id ComCcsdsConfig.BASE_ID_SBAND + 0x04000 + + instance spacePacketDeframer: Svc.Ccsds.SpacePacketDeframer base id ComCcsdsConfig.BASE_ID_SBAND + 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_SBAND + 0x07000 + + instance spacePacketFramer: Svc.Ccsds.SpacePacketFramer base id ComCcsdsConfig.BASE_ID_SBAND + 0x08000 + + instance apidManager: Svc.Ccsds.ApidManager base id ComCcsdsConfig.BASE_ID_SBAND + 0x09000 + + instance comStub: Svc.ComStub base id ComCcsdsConfig.BASE_ID_SBAND + 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 ComCcsdsSband diff --git a/FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp b/FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp new file mode 100644 index 00000000..dd033856 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp @@ -0,0 +1,7 @@ +#ifndef COMCCSDS_SBAND_PINGENTRIES_HPP +#define COMCCSDS_SBAND_PINGENTRIES_HPP +namespace PingEntries { +// No ping-enabled components in ComCcsds S-band subtopology +} + +#endif diff --git a/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp b/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp new file mode 100644 index 00000000..1b116f17 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp @@ -0,0 +1,21 @@ +#ifndef COMCCSDSSBANDSUBDEFS_HPP +#define COMCCSDSSBANDSUBDEFS_HPP + +#include +#include +#include + +#include "ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp" +#include "Svc/Subtopologies/ComCcsds/ComCcsdsConfig/FppConstantsAc.hpp" + +namespace ComCcsdsSband { +struct SubtopologyState { + // Empty - no external state needed for ComCcsdsSband subtopology +}; + +struct TopologyState { + SubtopologyState comCcsdsSband; +}; +} // namespace ComCcsdsSband + +#endif diff --git a/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md b/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md new file mode 100644 index 00000000..9e000e31 --- /dev/null +++ b/FprimeZephyrReference/ComCcsdsSband/docs/sdd.md @@ -0,0 +1,5 @@ +# ComCcsdsSband + +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/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index b899a717..28661e8d 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -13,5 +13,6 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ModeManager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/NullPrmDb/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/PowerMonitor/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ResetManager/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/SBand/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/StartupManager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Watchdog") diff --git a/FprimeZephyrReference/Components/SBand/CMakeLists.txt b/FprimeZephyrReference/Components/SBand/CMakeLists.txt new file mode 100644 index 00000000..24968493 --- /dev/null +++ b/FprimeZephyrReference/Components/SBand/CMakeLists.txt @@ -0,0 +1,42 @@ +#### +# 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}/SBand.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/SBand.cpp" + "${CMAKE_CURRENT_LIST_DIR}/FprimeHal.cpp" + DEPENDS + RadioLib +) + +add_subdirectory("${FPRIME_PROJECT_ROOT}/lib/RadioLib" "${CMAKE_BINARY_DIR}/RadioLib") + +# Disable compile warnings for RadioLib as they pollute the log and we can't do anything about them. +target_compile_options(RadioLib PRIVATE -w) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/SBand.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/SBandTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/SBandTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/SBand/FprimeHal.cpp b/FprimeZephyrReference/Components/SBand/FprimeHal.cpp new file mode 100644 index 00000000..516e484c --- /dev/null +++ b/FprimeZephyrReference/Components/SBand/FprimeHal.cpp @@ -0,0 +1,77 @@ +#define RADIOLIB_LOW_LEVEL 1 + +#include "FprimeHal.hpp" + +#include +#include +#include + +#include "SBand.hpp" +#include + +FprimeHal::FprimeHal(Components::SBand* component) : RadioLibHal(0, 0, 0, 0, 0, 0), m_component(component) {} + +void FprimeHal::init() {} + +void FprimeHal::term() {} + +void FprimeHal::pinMode(uint32_t pin, uint32_t mode) {} + +void FprimeHal::digitalWrite(uint32_t pin, uint32_t value) {} + +uint32_t FprimeHal::digitalRead(uint32_t pin) { + if (pin == 5) { + Fw::Logic irqState; + Drv::GpioStatus state = this->m_component->getIRQLine_out(0, irqState); + FW_ASSERT(state == Drv::GpioStatus::OP_OK); + if (irqState == Fw::Logic::HIGH) + return 1; + else + return 0; + } + return 0; +} + +void FprimeHal::attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) {} + +void FprimeHal::detachInterrupt(uint32_t interruptNum) {} + +void FprimeHal::delay(unsigned long ms) { + Os::Task::delay(Fw::TimeInterval(0, ms * 1000)); +} + +void FprimeHal::delayMicroseconds(unsigned long us) { + Os::Task::delay(Fw::TimeInterval(0, us)); +} + +unsigned long FprimeHal::millis() { + return k_uptime_get(); +} + +unsigned long FprimeHal::micros() { + return k_uptime_get() * 1000; +} + +long FprimeHal::pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) { + return 0; +} + +void FprimeHal::tone(uint32_t pin, unsigned int frequency, unsigned long duration) {} + +void FprimeHal::noTone(uint32_t pin) {} + +void FprimeHal::spiBegin() {} + +void FprimeHal::spiBeginTransaction() {} + +void FprimeHal::spiTransfer(uint8_t* out, size_t len, uint8_t* in) { + Fw::Buffer writeBuffer(out, len); + Fw::Buffer readBuffer(in, len); + this->m_component->spiSend_out(0, writeBuffer, readBuffer); +} + +void FprimeHal::yield() {} + +void FprimeHal::spiEndTransaction() {} + +void FprimeHal::spiEnd() {} diff --git a/FprimeZephyrReference/Components/SBand/FprimeHal.hpp b/FprimeZephyrReference/Components/SBand/FprimeHal.hpp new file mode 100644 index 00000000..a071e485 --- /dev/null +++ b/FprimeZephyrReference/Components/SBand/FprimeHal.hpp @@ -0,0 +1,60 @@ +#ifndef FPRIME_HAL_H +#define FPRIME_HAL_H + +#include + +namespace Components { +class SBand; +} // namespace Components + +// the HAL must inherit from the base RadioLibHal class +// and implement all of its virtual methods +class FprimeHal : public RadioLibHal { + public: + explicit FprimeHal(Components::SBand* component); + + void init() override; + + void term() override; + + void pinMode(uint32_t pin, uint32_t mode) override; + + void digitalWrite(uint32_t pin, uint32_t value) override; + + uint32_t digitalRead(uint32_t pin) override; + + void attachInterrupt(uint32_t interruptNum, void (*interruptCb)(void), uint32_t mode) override; + + void detachInterrupt(uint32_t interruptNum) override; + + void delay(unsigned long ms) override; + + void delayMicroseconds(unsigned long us) override; + + unsigned long millis() override; + + unsigned long micros() override; + + long pulseIn(uint32_t pin, uint32_t state, unsigned long timeout) override; + + void tone(uint32_t pin, unsigned int frequency, unsigned long duration = 0) override; + + void noTone(uint32_t pin) override; + + void spiBegin(); + + void spiBeginTransaction(); + + void spiTransfer(uint8_t* out, size_t len, uint8_t* in); + + void yield() override; + + void spiEndTransaction(); + + void spiEnd(); + + private: + Components::SBand* m_component; +}; + +#endif diff --git a/FprimeZephyrReference/Components/SBand/SBand.cpp b/FprimeZephyrReference/Components/SBand/SBand.cpp new file mode 100644 index 00000000..e980615d --- /dev/null +++ b/FprimeZephyrReference/Components/SBand/SBand.cpp @@ -0,0 +1,243 @@ +// ====================================================================== +// \title SBand.cpp +// \author jrpear +// \brief cpp file for SBand component implementation class +// ====================================================================== + +#define RADIOLIB_LOW_LEVEL 1 + +#include "FprimeZephyrReference/Components/SBand/SBand.hpp" + +#include + +#include +#include + +#include "FprimeHal.hpp" + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +SBand ::SBand(const char* const compName) + : SBandComponentBase(compName), m_rlb_hal(this), m_rlb_module(&m_rlb_hal, 0, 5, 0), m_rlb_radio(&m_rlb_module) {} + +SBand ::~SBand() {} + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void SBand ::run_handler(FwIndexType portNum, U32 context) { + // Only process if radio is configured + if (!m_configured) { + return; + } + + // Queue RX handler only if not already queued + if (!m_rxHandlerQueued) { + m_rxHandlerQueued = true; + this->deferredRxHandler_internalInterfaceInvoke(); + } +} + +void SBand ::deferredRxHandler_internalInterfaceHandler() { + // Check IRQ status + uint16_t irqStatus = this->m_rlb_radio.getIrqStatus(); + + // Only process if RX_DONE + if (irqStatus & RADIOLIB_SX128X_IRQ_RX_DONE) { + // Process received data + SX1280* radio = &this->m_rlb_radio; + uint8_t data[256] = {0}; + size_t len = radio->getPacketLength(); + int16_t state = radio->readData(data, len); + + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + } else { + Fw::Buffer buffer = this->allocate_out(0, static_cast(len)); + if (buffer.isValid()) { + (void)::memcpy(buffer.getData(), data, len); + ComCfg::FrameContext frameContext; + this->dataOut_out(0, buffer, frameContext); + + // Log RSSI and SNR for received packet + float rssi = radio->getRSSI(); + float snr = radio->getSNR(); + this->tlmWrite_LastRssi(rssi); + this->tlmWrite_LastSnr(snr); + + // Clear throttled warnings on success + this->log_WARNING_HI_RadioLibFailed_ThrottleClear(); + this->log_WARNING_HI_AllocationFailed_ThrottleClear(); + } else { + this->log_WARNING_HI_AllocationFailed(static_cast(len)); + } + } + + // Re-enable receive mode + state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + } + } + + // Clear the queued flag + m_rxHandlerQueued = false; +} + +void SBand ::deferredTxHandler_internalInterfaceHandler(const Fw::Buffer& data, const ComCfg::FrameContext& context) { + Fw::Success returnStatus = Fw::Success::FAILURE; + + // Enable transmit mode + Status status = this->enableTx(); + if (status == Status::SUCCESS) { + // Transmit data + int16_t state = this->m_rlb_radio.transmit(data.getData(), data.getSize()); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + returnStatus = Fw::Success::FAILURE; + } else { + returnStatus = Fw::Success::SUCCESS; + // Clear throttled warnings on success + this->log_WARNING_HI_RadioLibFailed_ThrottleClear(); + } + } + + // Return buffer and status (need mutable copy for dataReturnOut_out) + Fw::Buffer mutableData = data; + this->dataReturnOut_out(0, mutableData, context); + this->comStatusOut_out(0, returnStatus); + + // Re-enable RX mode after transmission + status = this->enableRx(); + // enableRx will log RadioLibFailed internally if it fails +} + +// ---------------------------------------------------------------------- +// Handler implementations for Com interface +// ---------------------------------------------------------------------- + +void SBand ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { + // Only process if radio is configured + if (!m_configured) { + this->log_WARNING_HI_RadioNotConfigured(); + Fw::Success failureStatus = Fw::Success::FAILURE; + this->dataReturnOut_out(0, data, context); + this->comStatusOut_out(0, failureStatus); + return; + } + + // Queue deferred handler to perform transmission + this->deferredTxHandler_internalInterfaceInvoke(data, context); +} + +void SBand ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { + // Deallocate the buffer + this->deallocate_out(0, data); +} + +SBand::Status SBand ::enableRx() { + this->txEnable_out(0, Fw::Logic::LOW); + this->rxEnable_out(0, Fw::Logic::HIGH); + + SX1280* radio = &this->m_rlb_radio; + + int16_t state = radio->standby(); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + state = radio->startReceive(RADIOLIB_SX128X_RX_TIMEOUT_INF); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + return Status::SUCCESS; +} + +SBand::Status SBand ::enableTx() { + this->rxEnable_out(0, Fw::Logic::LOW); + this->txEnable_out(0, Fw::Logic::HIGH); + + SX1280* radio = &this->m_rlb_radio; + + int16_t state = radio->standby(); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + return Status::SUCCESS; +} + +SBand::Status SBand ::configure_radio() { + int16_t state = this->m_rlb_radio.begin(); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + state = this->m_rlb_radio.setOutputPower(13); // 13dB is max + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + // Match modulation parameters to CircuitPython defaults + state = this->m_rlb_radio.setSpreadingFactor(7); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + state = this->m_rlb_radio.setBandwidth(406.25); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + state = this->m_rlb_radio.setCodingRate(5); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + state = this->m_rlb_radio.setPacketParamsLoRa(12, RADIOLIB_SX128X_LORA_HEADER_EXPLICIT, 255, + RADIOLIB_SX128X_LORA_CRC_ON, RADIOLIB_SX128X_LORA_IQ_STANDARD); + if (state != RADIOLIB_ERR_NONE) { + this->log_WARNING_HI_RadioLibFailed(state); + return Status::ERROR; + } + + return Status::SUCCESS; +} + +SBand::Status SBand ::configureRadio() { + Status config_status = this->configure_radio(); + if (config_status != Status::SUCCESS) { + // configure_radio logs RadioLibFailed internally + return Status::ERROR; + } + + // Mark as configured + m_configured = true; + + // Enable RX mode + Status rx_status = this->enableRx(); + if (rx_status != Status::SUCCESS) { + // enableRx logs RadioLibFailed internally + return Status::ERROR; + } + + // Send success status + Fw::Success status = Fw::Success::SUCCESS; + this->comStatusOut_out(0, status); + + return Status::SUCCESS; +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/SBand/SBand.fpp b/FprimeZephyrReference/Components/SBand/SBand.fpp new file mode 100644 index 00000000..e699f2a2 --- /dev/null +++ b/FprimeZephyrReference/Components/SBand/SBand.fpp @@ -0,0 +1,93 @@ + + +module Components { + @ Component for F Prime FSW framework. + active component SBand { + + ############################################################################## + #### Uncomment the following examples to start customizing your component #### + ############################################################################## + + @ Port receiving calls from the rate group + sync input port run: Svc.Sched + + @ Internal port for deferred RX processing + internal port deferredRxHandler() priority 10 + + @ Internal port for deferred TX processing + internal port deferredTxHandler( + data: Fw.Buffer + context: ComCfg.FrameContext + ) priority 10 + + @ Import Communication Interface + import Svc.Com + + @ Import the allocation interface + import Svc.BufferAllocation + + @ SPI Output Port + output port spiSend: Drv.SpiReadWrite + + @ Radio Module Reset GPIO + output port resetSend: Drv.GpioWrite + + @ S-Band TX Enable GPIO (separate from reset) + output port txEnable: Drv.GpioWrite + + @ S-Band RX Enable GPIO + output port rxEnable: Drv.GpioWrite + + @ S-Band IRQ Line + output port getIRQLine: Drv.GpioRead + + @ Event to indicate RadioLib call failure + event RadioLibFailed(error: I16) severity warning high \ + format "SBand RadioLib call failed, error: {}" throttle 2 + + @ Event to indicate allocation failure + event AllocationFailed(allocation_size: FwSizeType) severity warning high \ + format "Failed to allocate buffer of: {} bytes" throttle 2 + + @ Event to indicate radio not configured + event RadioNotConfigured() severity warning high \ + format "Radio not configured, operation ignored" throttle 3 + + @ Last received RSSI (if available) + telemetry LastRssi: F32 update on change + + @ Last received SNR (if available) + telemetry LastSnr: F32 update on change + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + @ Port for sending command registrations + command reg port cmdRegOut + + @ Port for receiving commands + command recv port cmdIn + + @ Port for sending command responses + command resp port cmdResponseOut + + @ Port for sending textual representation of events + text event port logTextOut + + @ Port for sending events to downlink + event port logOut + + @ Port for sending telemetry channels to downlink + telemetry port tlmOut + + @ Port to return the value of a parameter + param get port prmGetOut + + @Port to set the value of a parameter + param set port prmSetOut + + } +} diff --git a/FprimeZephyrReference/Components/SBand/SBand.hpp b/FprimeZephyrReference/Components/SBand/SBand.hpp new file mode 100644 index 00000000..168e845d --- /dev/null +++ b/FprimeZephyrReference/Components/SBand/SBand.hpp @@ -0,0 +1,95 @@ +// ====================================================================== +// \title SBand.hpp +// \author jrpear +// \brief hpp file for SBand component implementation class +// ====================================================================== + +#ifndef Components_SBand_HPP +#define Components_SBand_HPP + +#include "FprimeHal.hpp" +#include "FprimeZephyrReference/Components/SBand/SBandComponentAc.hpp" + +namespace Components { + +class SBand final : public SBandComponentBase { + public: + // Status returned from various SBand operations + enum Status { ERROR, SUCCESS }; + + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct SBand object + SBand(const char* const compName //!< The component name + ); + + //! Destroy SBand object + ~SBand(); + + //! Configure the radio and start operation + Status configureRadio(); + + using SBandComponentBase::getIRQLine_out; + using SBandComponentBase::getTime; + using SBandComponentBase::spiSend_out; + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for run + //! + //! Port receiving calls from the rate group + void run_handler(FwIndexType portNum, //!< The port number + U32 context //!< The call order + ) override; + + //! Handler implementation for dataIn + //! + //! Data to be sent on the wire (coming in to the component) + void dataIn_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& data, + const ComCfg::FrameContext& context) override; + + //! Handler implementation for dataReturnIn + //! + //! Port receiving back ownership of buffer sent out on dataOut + void dataReturnIn_handler(FwIndexType portNum, //!< The port number + Fw::Buffer& data, + const ComCfg::FrameContext& context) override; + + //! Handler implementation for deferredRxHandler + //! + //! Internal async handler for processing received data + void deferredRxHandler_internalInterfaceHandler() override; + + //! Handler implementation for deferredTxHandler + //! + //! Internal async handler for processing transmitted data + void deferredTxHandler_internalInterfaceHandler(const Fw::Buffer& data, + const ComCfg::FrameContext& context) override; + + private: + // Configure the SX1280 radio (setup and parameter tuning) + Status configure_radio(); + + //! Enable receive mode + Status enableRx(); + + //! Enable transmit mode + Status enableTx(); + + private: + FprimeHal m_rlb_hal; //!< RadioLib HAL instance + Module m_rlb_module; //!< RadioLib Module instance + SX1280 m_rlb_radio; //!< RadioLib SX1280 radio instance + bool m_configured = false; //!< Flag indicating radio is configured + bool m_rxHandlerQueued = false; //!< Flag indicating RX handler is queued +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/Components/SBand/docs/sdd.md b/FprimeZephyrReference/Components/SBand/docs/sdd.md new file mode 100644 index 00000000..071c3bcd --- /dev/null +++ b/FprimeZephyrReference/Components/SBand/docs/sdd.md @@ -0,0 +1,62 @@ +# Components::SBand + +Wrapper for the [RadioLib SX1280 driver](https://github.com/jgromes/RadioLib). This component integrates into the F Prime communication stack. + +### Typical Usage + +The component should be connected in accordance with the [F Prime communication adapter interface](../../../../lib/fprime/docs/reference/communication-adapter-interface.md). + + +## Port Descriptions + +| Name | Description | +|---|---| +| run | Scheduler port called by rate group to check for received data | +| Svc.Com | Standard communication interface (dataIn, dataOut, dataReturnIn, dataReturnOut, comStatusIn, comStatusOut) | +| Svc.BufferAllocation | Buffer allocation interface (allocate, deallocate) | +| spiSend | SPI communication with the SX1280 radio | +| resetSend | GPIO control for radio module reset | +| txEnable | GPIO control for S-Band TX enable | +| rxEnable | GPIO control for S-Band RX enable | +| getIRQLine | GPIO read for S-Band IRQ line status | + +## Parameters +| Name | Description | +|---|---| +|---|---| + +## Commands +| Name | Description | +|---|---| +|---|---| + +## Events +| Name | Description | +|---|---| +| RadioLibFailed | RadioLib call failed with error code (throttled: 2) | +| AllocationFailed | Failed to allocate buffer for received data (throttled: 2) | +| RadioNotConfigured | Radio not configured, operation ignored (throttled: 3) | + +## Telemetry +| Name | Description | +|---|---| +| LastRssi | RSSI (Received Signal Strength Indicator) of last received packet in dBm | +| LastSnr | SNR (Signal-to-Noise Ratio) of last received packet in dB | + + +## Unit Tests + +| Name | Description | Output | Coverage | +|---|---|---|---| +|---|---|---|---| + +## Requirements +Add requirements in the chart below +| Name | Description | Validation | +|---|---|---| +|---|---|---| + +## Change Log +| Date | Description | +|---|---| +|---| Initial Draft | diff --git a/FprimeZephyrReference/ReferenceDeployment/Main.cpp b/FprimeZephyrReference/ReferenceDeployment/Main.cpp index c4d250c5..c2580b85 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Main.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Main.cpp @@ -14,6 +14,7 @@ const struct device* ina219Sys = DEVICE_DT_GET(DT_NODELABEL(ina219_0)); const struct device* ina219Sol = DEVICE_DT_GET(DT_NODELABEL(ina219_1)); const struct device* serial = DEVICE_DT_GET(DT_NODELABEL(cdc_acm_uart0)); const struct device* lora = DEVICE_DT_GET(DT_NODELABEL(lora0)); +const struct device* spi0 = DEVICE_DT_GET(DT_NODELABEL(spi0)); 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)); @@ -27,6 +28,7 @@ int main(int argc, char* argv[]) { Os::init(); // Object for communicating state to the topology ReferenceDeployment::TopologyState inputs; + inputs.spi0Device = spi0; inputs.ina219SysDevice = ina219Sys; inputs.ina219SolDevice = ina219Sol; inputs.loraDevice = lora; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 6161868a..a1f821dd 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -5,6 +5,8 @@ telemetry packets ReferenceDeploymentPackets { CdhCore.cmdDisp.CommandsDropped ComCcsds.comQueue.comQueueDepth ComCcsds.commsBufferManager.HiBuffs + ComCcsdsSband.comQueue.comQueueDepth + ComCcsdsSband.commsBufferManager.HiBuffs ComCcsdsUart.comQueue.comQueueDepth ComCcsdsUart.commsBufferManager.HiBuffs ReferenceDeployment.rateGroup10Hz.RgMaxTime @@ -23,6 +25,8 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.EmptyBuffs ReferenceDeployment.rateGroup10Hz.RgCycleSlips ReferenceDeployment.rateGroup1Hz.RgCycleSlips + ComCcsdsSband.commsBufferManager.NoBuffs + ComCcsdsSband.commsBufferManager.EmptyBuffs } @@ -34,6 +38,9 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.CurrBuffs ComCcsdsUart.comQueue.buffQueueDepth CdhCore.tlmSend.SendLevel + ComCcsdsSband.commsBufferManager.TotalBuffs + ComCcsdsSband.commsBufferManager.CurrBuffs + ComCcsdsSband.comQueue.buffQueueDepth } packet Version id 4 group 3 { @@ -86,6 +93,11 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.powerMonitor.TotalPowerGenerated } + packet SBand id 11 group 4 { + sband.LastRssi + sband.LastSnr + } + } omit { CdhCore.cmdDisp.CommandErrors # Only has one library, no custom versions diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index af8e02a4..ed60e279 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -12,6 +12,7 @@ #include #include +#include static const struct gpio_dt_spec ledGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(led0), gpios); static const struct gpio_dt_spec burnwire0Gpio = GPIO_DT_SPEC_GET(DT_NODELABEL(burnwire0), gpios); @@ -25,6 +26,10 @@ static const struct gpio_dt_spec face5LoadSwitchGpio = GPIO_DT_SPEC_GET(DT_NODEL static const struct gpio_dt_spec payloadPowerLoadSwitchGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(payload_pwr_enable), gpios); static const struct gpio_dt_spec payloadBatteryLoadSwitchGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(payload_batt_enable), gpios); +static const struct gpio_dt_spec sbandNrstGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_nrst), gpios); +static const struct gpio_dt_spec sbandRxEnGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_rx_en), gpios); +static const struct gpio_dt_spec sbandTxEnGpio = GPIO_DT_SPEC_GET(DT_NODELABEL(sband_tx_en), gpios); +static const struct gpio_dt_spec sbandTxEnIRQ = GPIO_DT_SPEC_GET(DT_NODELABEL(rf2_io1), gpios); // Allows easy reference to objects in FPP/autocoder required namespaces using namespace ReferenceDeployment; @@ -79,6 +84,10 @@ void configureTopology() { gpioface5LS.open(face5LoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioPayloadPowerLS.open(payloadPowerLoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioPayloadBatteryLS.open(payloadBatteryLoadSwitchGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandNrst.open(sbandNrstGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandRxEn.open(sbandRxEnGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandTxEn.open(sbandTxEnGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); + gpioSbandIRQ.open(sbandTxEnIRQ, Zephyr::ZephyrGpioDriver::GpioConfiguration::IN); } // Public functions for use in main program are namespaced with deployment name ReferenceDeployment @@ -111,6 +120,21 @@ void setupTopology(const TopologyState& state) { lora.start(state.loraDevice, Zephyr::TransmitState::DISABLED); comDriver.configure(state.uartDevice, state.baudRate); + static struct spi_cs_control cs_ctrl = { + .gpio = GPIO_DT_SPEC_GET_BY_IDX(DT_NODELABEL(spi0), cs_gpios, 0), + .delay = 0U, /* us to wait after asserting CS before transfer */ + .cs_is_gpio = true, + }; + + struct spi_config cfg = { + .frequency = 100000, // 100 KHz -- sx1280 has maximum 18.18 MHz -- there is a 12MHz oscillator on-board + .operation = SPI_WORD_SET(8), + .slave = 0, + .cs = cs_ctrl, + .word_delay = 0, + }; + spiDriver.configure(state.spi0Device, cfg); + sband.configureRadio(); lsm6dsoManager.configure(state.lsm6dsoDevice); lis2mdlManager.configure(state.lis2mdlDevice); ina219SysManager.configure(state.ina219SysDevice); diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp index 00b11005..724a93d7 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -7,12 +7,14 @@ #define REFERENCEDEPLOYMENT_REFERENCEDEPLOYMENTTOPOLOGYDEFS_HPP // Subtopology PingEntries includes +#include "FprimeZephyrReference/ComCcsdsSband/PingEntries.hpp" #include "Svc/Subtopologies/CdhCore/PingEntries.hpp" #include "Svc/Subtopologies/ComCcsds/PingEntries.hpp" #include "Svc/Subtopologies/DataProducts/PingEntries.hpp" #include "Svc/Subtopologies/FileHandling/PingEntries.hpp" // SubtopologyTopologyDefs includes +#include "FprimeZephyrReference/ComCcsdsSband/SubtopologyTopologyDefs.hpp" #include "Svc/Subtopologies/CdhCore/SubtopologyTopologyDefs.hpp" #include "Svc/Subtopologies/ComCcsds/SubtopologyTopologyDefs.hpp" #include "Svc/Subtopologies/FileHandling/SubtopologyTopologyDefs.hpp" @@ -69,17 +71,19 @@ namespace ReferenceDeployment { * autocoder. The contents are entirely up to the definition of the project. This deployment uses subtopologies. */ struct TopologyState { - const device* uartDevice; //!< UART device path for communication - const device* loraDevice; //!< LoRa device path for communication - U32 baudRate; //!< Baud rate for UART communication - CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore - ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds - FileHandling::SubtopologyState fileHandling; //!< Subtopology state for FileHandling - const device* ina219SysDevice; //!< device path for battery board ina219 - const device* ina219SolDevice; //!< device path for solar panel ina219 - const device* lsm6dsoDevice; //!< LSM6DSO device path for accelerometer/gyroscope - const device* lis2mdlDevice; //!< LIS2MDL device path for magnetometer - const device* rtcDevice; //!< RTC device path + const device* uartDevice; //!< UART device path for communication + const device* loraDevice; //!< LoRa device path for communication + const device* spi0Device; //!< Spi device path for s-band LoRa module + U32 baudRate; //!< Baud rate for UART communication + CdhCore::SubtopologyState cdhCore; //!< Subtopology state for CdhCore + ComCcsds::SubtopologyState comCcsds; //!< Subtopology state for ComCcsds + FileHandling::SubtopologyState fileHandling; //!< Subtopology state for FileHandling + ComCcsdsSband::SubtopologyState comCcsdsSband; //!< Subtopology state for ComCcsdsSband + const device* ina219SysDevice; //!< device path for battery board ina219 + const device* ina219SolDevice; //!< device path for solar panel ina219 + const device* lsm6dsoDevice; //!< LSM6DSO device path for accelerometer/gyroscope + const device* lis2mdlDevice; //!< LIS2MDL device path for magnetometer + const device* rtcDevice; //!< RTC device path }; namespace PingEntries = ::PingEntries; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 42bae188..a94499a6 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -90,6 +90,8 @@ module ReferenceDeployment { instance comDelay: Components.ComDelay base id 0x10025000 + instance comDelaySband: Components.ComDelay base id 0x10024000 + instance lora: Zephyr.LoRa base id 0x10026000 instance comSplitterEvents: Svc.ComSplitter base id 0x10027000 @@ -98,6 +100,21 @@ module ReferenceDeployment { instance antennaDeployer: Components.AntennaDeployer base id 0x10029000 + instance spiDriver: Zephyr.ZephyrSpiDriver base id 0x10050000 + + instance sband : Components.SBand base id 0x10051000 \ + queue size Default.QUEUE_SIZE \ + stack size Default.STACK_SIZE \ + priority 10 + + instance gpioSbandNrst: Zephyr.ZephyrGpioDriver base id 0x10052000 + + instance gpioSbandRxEn: Zephyr.ZephyrGpioDriver base id 0x10053000 + + instance gpioSbandTxEn: Zephyr.ZephyrGpioDriver base id 0x10054000 + + instance gpioSbandIRQ: Zephyr.ZephyrGpioDriver base id 0x10055000 + instance gpioface4LS: Zephyr.ZephyrGpioDriver base id 0x1002A000 instance gpioface0LS: Zephyr.ZephyrGpioDriver base id 0x1002B000 diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 0390534e..b88c9aef 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -18,6 +18,7 @@ module ReferenceDeployment { import ComCcsds.FramingSubtopology import ComCcsdsUart.Subtopology import FileHandling.Subtopology + import ComCcsdsSband.FramingSubtopology # ---------------------------------------------------------------------- # Instances used in the topology @@ -45,13 +46,19 @@ module ReferenceDeployment { instance lsm6dsoManager instance bootloaderTrigger instance comDelay + instance comDelaySband instance burnwire instance antennaDeployer instance comSplitterEvents instance comSplitterTelemetry # For UART sideband communication instance comDriver - + instance spiDriver + instance sband + instance gpioSbandNrst + instance gpioSbandRxEn + instance gpioSbandTxEn + instance gpioSbandIRQ instance face4LoadSwitch instance face0LoadSwitch instance face1LoadSwitch @@ -111,6 +118,32 @@ module ReferenceDeployment { cmdSeq.comCmdOut -> CdhCore.cmdDisp.seqCmdBuff CdhCore.cmdDisp.seqCmdStatus -> cmdSeq.cmdResponseIn + + + # Sband connections + comSplitterEvents.comOut-> ComCcsdsSband.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.EVENTS] + comSplitterTelemetry.comOut -> ComCcsdsSband.comQueue.comPacketQueueIn[ComCcsds.Ports_ComPacketQueue.TELEMETRY] + + ComCcsdsSband.fprimeRouter.commandOut -> CdhCore.cmdDisp.seqCmdBuff + CdhCore.cmdDisp.seqCmdStatus -> ComCcsdsSband.fprimeRouter.cmdResponseIn + + } + + + connections CommunicationsSBandRadio { + sband.allocate -> ComCcsdsSband.commsBufferManager.bufferGetCallee + sband.deallocate -> ComCcsdsSband.commsBufferManager.bufferSendIn + + # ComDriver <-> ComStub (Uplink) + sband.dataOut -> ComCcsdsSband.frameAccumulator.dataIn + ComCcsdsSband.frameAccumulator.dataReturnOut -> sband.dataReturnIn + + # ComStub <-> ComDriver (Downlink) + ComCcsdsSband.framer.dataOut -> sband.dataIn + sband.dataReturnOut -> ComCcsdsSband.framer.dataReturnIn + sband.comStatusOut -> comDelaySband.comStatusIn + comDelaySband.comStatusOut -> ComCcsdsSband.framer.comStatusIn + } connections CommunicationsRadio { @@ -156,6 +189,8 @@ module ReferenceDeployment { rateGroup10Hz.RateGroupMemberOut[2] -> ComCcsds.aggregator.timeout rateGroup10Hz.RateGroupMemberOut[3] -> FileHandling.fileManager.schedIn rateGroup10Hz.RateGroupMemberOut[4] -> cmdSeq.schedIn + rateGroup10Hz.RateGroupMemberOut[5] -> ComCcsdsSband.aggregator.timeout + rateGroup10Hz.RateGroupMemberOut[6] -> sband.run # Slow rate (1Hz) rate group rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1Hz] -> rateGroup1Hz.CycleIn @@ -166,6 +201,7 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[4] -> watchdog.run rateGroup1Hz.RateGroupMemberOut[5] -> imuManager.run rateGroup1Hz.RateGroupMemberOut[6] -> comDelay.run + rateGroup1Hz.RateGroupMemberOut[16] -> comDelaySband.run rateGroup1Hz.RateGroupMemberOut[7] -> burnwire.schedIn rateGroup1Hz.RateGroupMemberOut[8] -> antennaDeployer.schedIn rateGroup1Hz.RateGroupMemberOut[9] -> fsSpace.run @@ -173,7 +209,8 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[11] -> startupManager.run rateGroup1Hz.RateGroupMemberOut[12] -> powerMonitor.run rateGroup1Hz.RateGroupMemberOut[13] -> modeManager.run - + rateGroup1Hz.RateGroupMemberOut[14] -> ComCcsdsSband.comQueue.run + rateGroup1Hz.RateGroupMemberOut[15] -> ComCcsdsSband.commsBufferManager.schedIn } @@ -209,6 +246,14 @@ module ReferenceDeployment { imuManager.temperatureGet -> lsm6dsoManager.temperatureGet } + connections MyConnectionGraph { + sband.spiSend -> spiDriver.SpiReadWrite + sband.resetSend -> gpioSbandNrst.gpioWrite + sband.txEnable -> gpioSbandTxEn.gpioWrite + sband.rxEnable -> gpioSbandRxEn.gpioWrite + sband.getIRQLine -> gpioSbandIRQ.gpioRead + } + connections ComCcsds_FileHandling { # File Downlink <-> ComQueue FileHandling.fileDownlink.bufferSendOut -> ComCcsdsUart.comQueue.bufferQueueIn[ComCcsds.Ports_ComBufferQueue.FILE] @@ -217,9 +262,14 @@ module ReferenceDeployment { # Router <-> FileUplink ComCcsdsUart.fprimeRouter.fileOut -> FileHandling.fileUplink.bufferSendIn FileHandling.fileUplink.bufferSendOut -> ComCcsdsUart.fprimeRouter.fileBufferReturnIn + + # Router <-> FileUplink (S-band input only - file downlink uses UART only) + ComCcsdsSband.fprimeRouter.fileOut -> FileHandling.fileUplink.bufferSendIn } + + connections sysPowerMonitor { powerMonitor.sysVoltageGet -> ina219SysManager.voltageGet powerMonitor.sysCurrentGet -> ina219SysManager.currentGet diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index 8eeefb4b..267483c6 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -4,7 +4,7 @@ # ====================================================================== @ Number of rate group member output ports for ActiveRateGroup -constant ActiveRateGroupOutputPorts = 15 +constant ActiveRateGroupOutputPorts = 20 @ Number of rate group member output ports for PassiveRateGroup constant PassiveRateGroupOutputPorts = 10 @@ -13,7 +13,7 @@ constant PassiveRateGroupOutputPorts = 10 constant RateGroupDriverRateGroupPorts = 3 @ Used for command and registration ports -constant CmdDispatcherComponentCommandPorts = 30 +constant CmdDispatcherComponentCommandPorts = 35 @ Used for uplink/sequencer buffer/response ports constant CmdDispatcherSequencePorts = 5 diff --git a/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp b/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp index 9d0bddf8..c384a6f4 100644 --- a/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp +++ b/FprimeZephyrReference/project/config/ComCcsdsConfig.fpp @@ -2,6 +2,7 @@ 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 + constant BASE_ID_SBAND = 0x23000000 module QueueSizes { constant comQueue = 5 diff --git "a/FprimeZephyrReference/project/config/ComCcsdsConfig/CMakeLists.txt\342\200\216" "b/FprimeZephyrReference/project/config/ComCcsdsConfig/CMakeLists.txt\342\200\216" new file mode 100644 index 00000000..aad62bfe --- /dev/null +++ "b/FprimeZephyrReference/project/config/ComCcsdsConfig/CMakeLists.txt\342\200\216" @@ -0,0 +1,12 @@ +register_fprime_module( + EXCLUDE_FROM_ALL + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/ComCcsdsSubtopologyConfig.cpp" + HEADERS + "${CMAKE_CURRENT_LIST_DIR}/ComCcsdsSubtopologyConfig.hpp" + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/../ComCcsdsConfig.fpp" + DEPENDS + Fw_Types + INTERFACE +) diff --git a/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp new file mode 100644 index 00000000..c94d1689 --- /dev/null +++ b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.cpp @@ -0,0 +1,9 @@ +#include "ComCcsdsSubtopologyConfig.hpp" + +namespace ComCcsdsSband { +namespace Allocation { +// This instance can be changed to use a different allocator in the ComCcsdsSband Subtopology +Fw::MallocAllocator mallocatorInstance; +Fw::MemAllocator& memAllocator = mallocatorInstance; +} // namespace Allocation +} // namespace ComCcsdsSband diff --git a/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp new file mode 100644 index 00000000..e249853d --- /dev/null +++ b/FprimeZephyrReference/project/config/ComCcsdsConfig/ComCcsdsSubtopologyConfig.hpp @@ -0,0 +1,12 @@ +#ifndef COMCCSDSSUBTOPOLOGY_CONFIG_HPP +#define COMCCSDSSUBTOPOLOGY_CONFIG_HPP + +#include "Fw/Types/MallocAllocator.hpp" + +namespace ComCcsdsSband { +namespace Allocation { +extern Fw::MemAllocator& memAllocator; +} +} // namespace ComCcsdsSband + +#endif diff --git a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 635eaa3e..0232ff95 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 = 11; 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/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 6c2544d3..ea574a07 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 @@ -74,7 +74,7 @@ zephyr_udc0: &usbd { &spi0 { status = "okay"; - cs-gpios = <&gpio0 15 GPIO_ACTIVE_LOW>; + cs-gpios = <&gpio0 7 GPIO_ACTIVE_LOW>, <&gpio0 15 GPIO_ACTIVE_LOW>; pinctrl-0 = <&spi0_default>; pinctrl-names = "default"; sdhc0: sdhc@0 { @@ -495,7 +495,7 @@ zephyr_udc0: &usbd { }; }; -// GPIO Expander IO +// GPIO Names / { // Define the GPIO outputs based on your schematic @@ -561,7 +561,22 @@ zephyr_udc0: &usbd { gpios = <&mcp23017 3 GPIO_ACTIVE_HIGH>; // GPA3 label = "PAYLOAD_BATT_ENABLE"; }; - }; + + sband_nrst: sband-nrst { + gpios = <&gpio0 17 (GPIO_PULL_UP | GPIO_ACTIVE_LOW)>; + label = "SBAND_NRST"; + }; + + sband_rx_en: sband-rx-en { + gpios = <&gpio0 21 (GPIO_PULL_UP | GPIO_ACTIVE_HIGH)>; + label = "SBAND_RX_EN"; + }; + + sband_tx_en: sband-tx-en { + gpios = <&gpio0 22 (GPIO_PULL_UP | GPIO_ACTIVE_HIGH)>; + label = "SBAND_TX_EN"; + }; + }; // Define GPIO inputs gpio_inputs { diff --git a/circuit-python-lora-passthrough-feather/code.py b/circuit-python-lora-passthrough-feather/code.py index 7f394ab9..37376774 100644 --- a/circuit-python-lora-passthrough-feather/code.py +++ b/circuit-python-lora-passthrough-feather/code.py @@ -7,7 +7,7 @@ # import time -import adafruit_rfm9x +import adafruit_rfm.rfm9x import board import digitalio import usb_cdc @@ -17,7 +17,7 @@ CS = digitalio.DigitalInOut(board.RFM9X_CS) RESET = digitalio.DigitalInOut(board.RFM9X_RST) -rfm95 = adafruit_rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) +rfm95 = adafruit_rfm.rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) rfm95.spreading_factor = 8 rfm95.signal_bandwidth = 125000 rfm95.coding_rate = 5 diff --git a/circuit-python-lora-passthrough/code.py b/circuit-python-lora-passthrough/code.py index 4efd89f5..3554798f 100644 --- a/circuit-python-lora-passthrough/code.py +++ b/circuit-python-lora-passthrough/code.py @@ -7,7 +7,7 @@ import time -import adafruit_rfm9x +import adafruit_rfm.rfm9x import board import digitalio import usb_cdc @@ -17,7 +17,7 @@ CS = digitalio.DigitalInOut(board.SPI0_CS0) RESET = digitalio.DigitalInOut(board.RF1_RST) -rfm95 = adafruit_rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) +rfm95 = adafruit_rfm.rfm9x.RFM9x(board.SPI(), CS, RESET, RADIO_FREQ_MHZ) rfm95.spreading_factor = 8 rfm95.signal_bandwidth = 125000 rfm95.coding_rate = 5 diff --git a/circuit-python-sband-passthrough/boot.py b/circuit-python-sband-passthrough/boot.py new file mode 100644 index 00000000..8b54a07d --- /dev/null +++ b/circuit-python-sband-passthrough/boot.py @@ -0,0 +1,12 @@ +""" +boot.py + +This file helps to use an RP2350 CircuitPython environment as a passthrough for +interacting with the radio and F Prime GDS. The usb_cdc.enable() function creates a +USB serial endpoint that the GDS can connect to to receive all the data that is +streamed out. The function has to be called in boot.py before the USB device is enabled. +""" + +import usb_cdc + +usb_cdc.enable(console=True, data=True) diff --git a/circuit-python-sband-passthrough/code.py b/circuit-python-sband-passthrough/code.py new file mode 100644 index 00000000..3efd4801 --- /dev/null +++ b/circuit-python-sband-passthrough/code.py @@ -0,0 +1,357 @@ +""" +CircuitPython Feather RP2350 LoRa Radio forwarder + +This code will forward any received LoRa packets to the serial console (sys.stdout). It cycles through neo pixel colors +to indicate packet reception. +""" + +import os +import time + +import board +import digitalio +import usb_cdc # for passthrough at end of file +from lib.adafruit_mcp230xx.mcp23017 import ( + MCP23017, # This is Hacky V5a Devel Stuff### +) +from lib.adafruit_tca9548a import TCA9548A # This is Hacky V5a Devel Stuff### + +# from lib.pysquared.Big_Data import AllFaces ### This is Hacky V5a Devel Stuff### +from lib.pysquared.beacon import Beacon +from lib.pysquared.cdh import CommandDataHandler +from lib.pysquared.config.config import Config +from lib.pysquared.file_validation.manager.file_validation import FileValidationManager +from lib.pysquared.hardware.burnwire.manager.burnwire import BurnwireManager +from lib.pysquared.hardware.busio import _spi_init, initialize_i2c_bus +from lib.pysquared.hardware.digitalio import initialize_pin +from lib.pysquared.hardware.imu.manager.lsm6dsox import LSM6DSOXManager +from lib.pysquared.hardware.light_sensor.manager.veml7700 import VEML7700Manager +from lib.pysquared.hardware.load_switch.manager.loadswitch_manager import ( + LoadSwitchManager, +) +from lib.pysquared.hardware.magnetometer.manager.lis2mdl import LIS2MDLManager +from lib.pysquared.hardware.power_monitor.manager.ina219 import INA219Manager +from lib.pysquared.hardware.radio.manager.rfm9x import RFM9xManager +from lib.pysquared.hardware.radio.manager.sx1280 import SX1280Manager +from lib.pysquared.hardware.radio.packetizer.packet_manager import PacketManager +from lib.pysquared.hardware.temperature_sensor.manager.mcp9808 import MCP9808Manager +from lib.pysquared.logger import Logger +from lib.pysquared.nvm.counter import Counter +from lib.pysquared.protos.power_monitor import PowerMonitorProto +from lib.pysquared.rtc.manager.microcontroller import MicrocontrollerManager +from lib.pysquared.sleep_helper import SleepHelper +from lib.pysquared.watchdog import Watchdog +from version import __version__ + +rtc = MicrocontrollerManager() + + +logger: Logger = Logger( + error_counter=Counter(0), + colorized=False, +) + +logger.info( + "Booting", + hardware_version=os.uname().version, # type: ignore[attr-defined] + software_version=__version__, +) + + +def get_temp(sensor): + """ + Get temperature + """ + for i in range(1000): + print(sensor.get_temperature().value) + time.sleep(0.1) + + +watchdog = Watchdog(logger, board.WDT_WDI) +watchdog.pet() + +logger.debug("Initializing Config") +config: Config = Config("config.json") + +mux_reset = initialize_pin(logger, board.MUX_RESET, digitalio.Direction.OUTPUT, False) + +# TODO(nateinaction): fix spi init +spi0 = _spi_init( + logger, + board.SPI0_SCK, + board.SPI0_MOSI, + board.SPI0_MISO, +) + +spi1 = _spi_init( + logger, + board.SPI1_SCK, + board.SPI1_MOSI, + board.SPI1_MISO, +) + +sband_radio = SX1280Manager( + logger, + config.radio, + spi1, + initialize_pin(logger, board.SPI1_CS0, digitalio.Direction.OUTPUT, True), + initialize_pin(logger, board.RF2_RST, digitalio.Direction.OUTPUT, True), + initialize_pin(logger, board.RF2_IO0, digitalio.Direction.OUTPUT, True), + 2.4, + initialize_pin(logger, board.RF2_TX_EN, digitalio.Direction.OUTPUT, False), + initialize_pin(logger, board.RF2_RX_EN, digitalio.Direction.OUTPUT, False), +) + +i2c1 = initialize_i2c_bus( + logger, + board.SCL1, + board.SDA1, + 100000, +) + +i2c0 = initialize_i2c_bus( + logger, + board.SCL0, + board.SDA0, + 100000, +) + +sleep_helper = SleepHelper(logger, config, watchdog) + +uhf_radio = RFM9xManager( + logger, + config.radio, + spi0, + initialize_pin(logger, board.SPI0_CS0, digitalio.Direction.OUTPUT, True), + initialize_pin(logger, board.RF1_RST, digitalio.Direction.OUTPUT, True), +) + +magnetometer = LIS2MDLManager(logger, i2c1) + +imu = LSM6DSOXManager(logger, i2c1, 0x6B) + +uhf_packet_manager = PacketManager( + logger, + uhf_radio, + config.radio.license, + Counter(2), + 0.2, +) + +cdh = CommandDataHandler(logger, config, uhf_packet_manager) + +beacon = Beacon( + logger, + config.cubesat_name, + uhf_packet_manager, + time.monotonic(), + imu, + magnetometer, + uhf_radio, + sband_radio, +) + +## Initialize the MCP23017 GPIO Expander and its pins ## +GPIO_RESET = initialize_pin( + logger, board.GPIO_EXPANDER_RESET, digitalio.Direction.OUTPUT, True +) +mcp = MCP23017(i2c1) + +# This sets up all of the GPIO pins on the MCP23017 +FACE4_ENABLE = mcp.get_pin(8) +FACE0_ENABLE = mcp.get_pin(9) +FACE1_ENABLE = mcp.get_pin(10) +FACE2_ENABLE = mcp.get_pin(11) +FACE3_ENABLE = mcp.get_pin(12) +ENAB_RF = mcp.get_pin(13) +VBUS_RESET = mcp.get_pin(14) +SPI0_CS1 = mcp.get_pin(15) +ENABLE_HEATER = mcp.get_pin(0) +PAYLOAD_PWR_ENABLE = mcp.get_pin(1) +Z_GPIO0 = mcp.get_pin(2) +Z_GPIO1 = mcp.get_pin(3) +RF2_IO2 = mcp.get_pin(4) +RF2_IO1 = mcp.get_pin(5) + +# This defines the direction of the GPIO pins +FACE4_ENABLE.direction = digitalio.Direction.OUTPUT +FACE0_ENABLE.direction = digitalio.Direction.OUTPUT +FACE1_ENABLE.direction = digitalio.Direction.OUTPUT +FACE2_ENABLE.direction = digitalio.Direction.OUTPUT +FACE3_ENABLE.direction = digitalio.Direction.OUTPUT +ENAB_RF.direction = digitalio.Direction.OUTPUT +VBUS_RESET.direction = digitalio.Direction.OUTPUT +ENABLE_HEATER.direction = digitalio.Direction.OUTPUT +PAYLOAD_PWR_ENABLE.direction = digitalio.Direction.OUTPUT + + +load_switch_0 = LoadSwitchManager(FACE0_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_1 = LoadSwitchManager(FACE1_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_2 = LoadSwitchManager(FACE2_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_3 = LoadSwitchManager(FACE3_ENABLE, True) # type: ignore , upstream on mcp TODO +load_switch_4 = LoadSwitchManager(FACE4_ENABLE, True) # type: ignore , upstream on mcp TODO + + +# Face Control Helper Functions +def all_faces_off(): + """ + This function turns off all of the faces. Note the load switches are disabled low. + """ + load_switch_0.disable_load() + load_switch_1.disable_load() + load_switch_2.disable_load() + load_switch_3.disable_load() + load_switch_4.disable_load() + + +def all_faces_on(): + """ + This function turns on all of the faces. Note the load switches are enabled high. + """ + load_switch_0.enable_load() + load_switch_1.enable_load() + load_switch_2.enable_load() + load_switch_3.enable_load() + load_switch_4.enable_load() + + +file_validator = FileValidationManager(logger) + +## Face Sensor Stuff ## + +# This is the TCA9548A I2C Multiplexer +all_faces_on() +mux_reset.value = True +tca = TCA9548A(i2c1, address=int(0x77)) + + +# Light Sensors +light_sensors = [] +try: + sensor = VEML7700Manager(logger, tca[0]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 0 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[1]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 1 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[2]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 2 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[3]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 3 failed to initialize") + light_sensors.append(None) +try: + sensor = VEML7700Manager(logger, tca[4]) + light_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Light sensor 4 failed to initialize") + light_sensors.append(None) + + +# Onboard Temp Sensors +temp_sensors = [] + +# Direct I2C sensors +try: + temp_sensor5 = MCP9808Manager(logger, i2c0, addr=25) # Antenna Board +except Exception: + logger.debug("WARNING!!! Temp sensor 5 (Antenna Board) failed") + temp_sensor5 = None +temp_sensors.append(temp_sensor5) + +try: + temp_sensor6 = MCP9808Manager(logger, i2c1, addr=27) # Flight Controller Board +except Exception: + logger.debug("WARNING!!! Temp sensor 6 (Flight Controller Board) failed") + temp_sensor6 = None +temp_sensors.append(temp_sensor6) + +# TCA-connected temp sensors +try: + sensor = MCP9808Manager(logger, tca[0], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor (TCA[0]) failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[1], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor 1]) failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[2], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor 2 failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[3], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor3 failed") + temp_sensors.append(None) +try: + sensor = MCP9808Manager(logger, tca[4], addr=27) + temp_sensors.append(sensor) +except Exception: + logger.debug("WARNING!!! Temp sensor 4 failed") + temp_sensors.append(None) + +battery_power_monitor: PowerMonitorProto = INA219Manager(logger, i2c1, 0x40) +solar_power_monitor: PowerMonitorProto = INA219Manager(logger, i2c1, 0x44) + +## Init Misc Pins ## +burnwire_heater_enable = initialize_pin( + logger, board.FIRE_DEPLOY1_A, digitalio.Direction.OUTPUT, False +) +burnwire1_fire = initialize_pin( + logger, board.FIRE_DEPLOY1_B, digitalio.Direction.OUTPUT, False +) + +## Initializing the Burn Wire ## +antenna_deployment = BurnwireManager( + logger, burnwire_heater_enable, burnwire1_fire, enable_logic=True +) + +# END `repl.py` copy + +time_start = time.time() +packet_count = 0 +last_packet = None + +print("[INFO] LoRa Receiver receiving packets") +while True: + # Returns the last packet received if there is no new packet + packet = sband_radio._radio.receive() + + # Only forward `packet` if it is indeed a new packet + # - Every packet is unique due to a unique counter which is prepended to + # each packet + # - The logical packet never spans more than one physical packet as + # packet size is hard-coded to 252 bytes on the Fprime board + if packet != last_packet: + usb_cdc.data.write(packet) + packet_count += 1 + last_packet = packet + time_delta = time.time() - time_start + if time_delta > 10: + print(f"[INFO] Packets received: {packet_count}") + time_start = time.time() + # `usb_cdc.data` is None until board is hard reset (after reflashing `boot.py`) + data = usb_cdc.data.read(usb_cdc.data.in_waiting) + if len(data) > 0: + print("Sending packet") + sband_radio._radio.send(data, header=False) + time.sleep(1) diff --git a/lib/RadioLib b/lib/RadioLib new file mode 160000 index 00000000..3a5dac09 --- /dev/null +++ b/lib/RadioLib @@ -0,0 +1 @@ +Subproject commit 3a5dac095d9f4c823fdc8a9e8f8e427244134981 diff --git a/lib/fprime-zephyr b/lib/fprime-zephyr index 66ea1412..d511ccaf 160000 --- a/lib/fprime-zephyr +++ b/lib/fprime-zephyr @@ -1 +1 @@ -Subproject commit 66ea141240a6cb0f4abab2815a8e41bc4b144b81 +Subproject commit d511ccaf7ffd41413e595a74f22f7424cab5dbb5