diff --git a/.codespell-ignore-words.txt b/.codespell-ignore-words.txt index 3d64f1a1..41db8f22 100644 --- a/.codespell-ignore-words.txt +++ b/.codespell-ignore-words.txt @@ -1,3 +1,4 @@ +ALS comIn Ines rsource diff --git a/FprimeZephyrReference/Components/ADCS/ADCS.cpp b/FprimeZephyrReference/Components/ADCS/ADCS.cpp new file mode 100644 index 00000000..2c3b4649 --- /dev/null +++ b/FprimeZephyrReference/Components/ADCS/ADCS.cpp @@ -0,0 +1,43 @@ +// ====================================================================== +// \title ADCS.cpp +// \brief cpp file for ADCS component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/ADCS/ADCS.hpp" + +#include + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +ADCS::ADCS(const char* const compName) : ADCSComponentBase(compName) {} + +ADCS::~ADCS() {} + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void ADCS::run_handler(FwIndexType portNum, U32 context) { + Fw::Success condition; + + // Visible light + for (FwIndexType i = 0; i < this->getNum_visibleLightGet_OutputPorts(); i++) { + this->visibleLightGet_out(i, condition); + } + + // Infra-red light + for (FwIndexType i = 0; i < this->getNum_infraRedLightGet_OutputPorts(); i++) { + this->infraRedLightGet_out(i, condition); + } + + // Ambient light + for (FwIndexType i = 0; i < this->getNum_ambientLightGet_OutputPorts(); i++) { + this->ambientLightGet_out(i, condition); + } +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/ADCS/ADCS.fpp b/FprimeZephyrReference/Components/ADCS/ADCS.fpp new file mode 100644 index 00000000..8315f709 --- /dev/null +++ b/FprimeZephyrReference/Components/ADCS/ADCS.fpp @@ -0,0 +1,30 @@ +module Components { + @ Attitude Determination and Control Component for F Prime FSW framework. + passive component ADCS { + sync input port run: Svc.Sched + + @ Port for visible light from the light sensors + output port visibleLightGet: [6] Drv.lightGet + + @Port for infra-red light from the light sensors + output port infraRedLightGet: [6] Drv.lightGet + + @Port for ambient light from the light sensors + output port ambientLightGet: [6] Drv.lightGet + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + @ Port for emitting telemetry + telemetry port tlmOut + + @ Port for emitting events + event port logOut + + @ Port for emitting text events + text event port logTextOut + } +} diff --git a/FprimeZephyrReference/Components/ADCS/ADCS.hpp b/FprimeZephyrReference/Components/ADCS/ADCS.hpp new file mode 100644 index 00000000..077815fb --- /dev/null +++ b/FprimeZephyrReference/Components/ADCS/ADCS.hpp @@ -0,0 +1,41 @@ +// ====================================================================== +// \title ADCS.hpp +// \brief hpp file for ADCS component implementation class +// ====================================================================== + +#ifndef Components_ADCS_HPP +#define Components_ADCS_HPP + +#include "FprimeZephyrReference/Components/ADCS/ADCSComponentAc.hpp" + +namespace Components { + +class ADCS final : public ADCSComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct ADCS object + ADCS(const char* const compName //!< The component name + ); + + //! Destroy ADCS object + ~ADCS(); + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for run + //! + //! Scheduled port for periodic temperature reading + void run_handler(FwIndexType portNum, //!< The port number + U32 context //!< The call order + ) override; +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/Components/ADCS/CMakeLists.txt b/FprimeZephyrReference/Components/ADCS/CMakeLists.txt new file mode 100644 index 00000000..fe80bbbf --- /dev/null +++ b/FprimeZephyrReference/Components/ADCS/CMakeLists.txt @@ -0,0 +1,18 @@ +#### +# 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/ +# +#### + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/ADCS.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/ADCS.cpp" +) diff --git a/FprimeZephyrReference/Components/CMakeLists.txt b/FprimeZephyrReference/Components/CMakeLists.txt index b899a717..a3589964 100644 --- a/FprimeZephyrReference/Components/CMakeLists.txt +++ b/FprimeZephyrReference/Components/CMakeLists.txt @@ -1,5 +1,6 @@ # Include project-wide components here +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ADCS/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/AntennaDeployer/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/BootloaderTrigger/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Burnwire/") @@ -14,4 +15,5 @@ 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}/StartupManager/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ThermalManager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Watchdog") diff --git a/FprimeZephyrReference/Components/Drv/CMakeLists.txt b/FprimeZephyrReference/Components/Drv/CMakeLists.txt index efcd2f90..7a38cbfe 100644 --- a/FprimeZephyrReference/Components/Drv/CMakeLists.txt +++ b/FprimeZephyrReference/Components/Drv/CMakeLists.txt @@ -2,4 +2,6 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Ina219Manager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Lis2mdlManager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Lsm6dsoManager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/RtcManager") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Tmp112Manager/") add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Types/") +add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Veml6031Manager/") diff --git a/FprimeZephyrReference/Components/Drv/Tmp112Manager/CMakeLists.txt b/FprimeZephyrReference/Components/Drv/Tmp112Manager/CMakeLists.txt new file mode 100644 index 00000000..30ae031a --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Tmp112Manager/CMakeLists.txt @@ -0,0 +1,36 @@ +#### +# F Prime CMakeLists.txt: +# +# SOURCES: list of source files (to be compiled) +# AUTOCODER_INPUTS: list of files to be passed to the autocoders +# DEPENDS: list of libraries that this module depends on +# +# More information in the F´ CMake API documentation: +# https://fprime.jpl.nasa.gov/latest/docs/reference/api/cmake/API/ +# +#### + +# Module names are derived from the path from the nearest project/library/framework +# root when not specifically overridden by the developer. i.e. The module defined by +# `Ref/SignalGen/CMakeLists.txt` will be named `Ref_SignalGen`. + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/Tmp112Manager.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/Tmp112Manager.cpp" +# DEPENDS +# MyPackage_MyOtherModule +) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/Tmp112Manager.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Tmp112ManagerTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Tmp112ManagerTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.cpp b/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.cpp new file mode 100644 index 00000000..10f5d5cb --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.cpp @@ -0,0 +1,165 @@ +// ====================================================================== +// \title Tmp112Manager.cpp +// \brief cpp file for Tmp112Manager component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/Drv/Tmp112Manager/Tmp112Manager.hpp" + +#include +#include + +#include + +namespace Drv { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +Tmp112Manager ::Tmp112Manager(const char* const compName) : Tmp112ManagerComponentBase(compName) {} + +Tmp112Manager ::~Tmp112Manager() {} + +// ---------------------------------------------------------------------- +// Helper methods +// ---------------------------------------------------------------------- + +void Tmp112Manager::configure(const struct device* tca, + const struct device* mux, + const struct device* dev, + bool loadSwitchCheck) { + this->m_tca = tca; + this->m_mux = mux; + this->m_dev = dev; + // TODO(nateinaction): Abstract load switch check, perhaps wrap this component and only use when needed? + this->m_load_switch_check = loadSwitchCheck; +} + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +Fw::Success Tmp112Manager ::loadSwitchStateChanged_handler(FwIndexType portNum, const Fw::On& state) { + // Store the load switch state + this->m_load_switch_state = state; + + // If the load switch is off, deinitialize the device + if (this->m_load_switch_state == Fw::On::OFF) { + return this->deinitializeDevice(); + } + + // If the load switch is on, set the timeout + // We only consider the load switch to be fully on after a timeout period + this->m_load_switch_on_timeout = this->getTime(); + this->m_load_switch_on_timeout.add(1, 0); + + return Fw::Success::SUCCESS; +} + +F64 Tmp112Manager ::temperatureGet_handler(FwIndexType portNum, Fw::Success& condition) { + condition = Fw::Success::FAILURE; + + if (!this->initializeDevice()) { + return 0; + } + + int rc = sensor_sample_fetch_chan(this->m_dev, SENSOR_CHAN_AMBIENT_TEMP); + if (rc != 0) { + this->log_WARNING_HI_SensorSampleFetchFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorSampleFetchFailed_ThrottleClear(); + + struct sensor_value val; + rc = sensor_channel_get(this->m_dev, SENSOR_CHAN_AMBIENT_TEMP, &val); + if (rc != 0) { + this->log_WARNING_HI_SensorChannelGetFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorChannelGetFailed_ThrottleClear(); + + F64 temp = sensor_value_to_double(&val); + + this->tlmWrite_Temperature(temp); + + condition = Fw::Success::SUCCESS; + return temp; +} + +bool Tmp112Manager ::isDeviceInitialized() { + if (!this->m_dev) { + this->log_WARNING_HI_DeviceNil(); + return false; + } + this->log_WARNING_HI_DeviceNil_ThrottleClear(); + + if (!this->m_dev->state) { + this->log_WARNING_HI_DeviceStateNil(); + return false; + } + this->log_WARNING_HI_DeviceStateNil_ThrottleClear(); + + return this->m_dev->state->initialized; +} + +Fw::Success Tmp112Manager ::initializeDevice() { + if (this->isDeviceInitialized()) { + if (!device_is_ready(this->m_dev)) { + this->log_WARNING_HI_DeviceNotReady(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceNotReady_ThrottleClear(); + return Fw::Success::SUCCESS; + } + + if (!device_is_ready(this->m_tca)) { + this->log_WARNING_HI_TcaUnhealthy(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_TcaUnhealthy_ThrottleClear(); + + if (!device_is_ready(this->m_mux)) { + this->log_WARNING_HI_MuxUnhealthy(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_MuxUnhealthy_ThrottleClear(); + + if (!this->loadSwitchReady()) { + this->log_WARNING_HI_LoadSwitchNotReady(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_LoadSwitchNotReady_ThrottleClear(); + + int rc = device_init(this->m_dev); + if (rc < 0) { + this->log_WARNING_HI_DeviceInitFailed(rc); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceInitFailed_ThrottleClear(); + + return Fw::Success::SUCCESS; +} + +Fw::Success Tmp112Manager ::deinitializeDevice() { + if (!this->m_dev) { + this->log_WARNING_HI_DeviceNil(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceNil_ThrottleClear(); + + if (!this->m_dev->state) { + this->log_WARNING_HI_DeviceStateNil(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceStateNil_ThrottleClear(); + + this->m_dev->state->initialized = false; + return Fw::Success::SUCCESS; +} + +bool Tmp112Manager ::loadSwitchReady() { + return (this->m_load_switch_state == Fw::On::ON && this->getTime() >= this->m_load_switch_on_timeout) || + !this->m_load_switch_check; +} + +} // namespace Drv diff --git a/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.fpp b/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.fpp new file mode 100644 index 00000000..81173781 --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.fpp @@ -0,0 +1,63 @@ +module Drv { + port temperatureGet(ref condition: Fw.Success) -> F64 +} + +module Drv { + @ Manager for TMP112 device + passive component Tmp112Manager { + # Ports + @ Port to read the temperature in degrees Celsius + sync input port temperatureGet: temperatureGet + + @ Port to initialize and deinitialize the TMP112 device on load switch state change + sync input port loadSwitchStateChanged: Components.loadSwitchStateChanged + + # Telemetry channels + + @ Telemetry channel for temperature in degrees Celsius + telemetry Temperature: F64 + + @ Event for reporting TMP112 not ready error + event DeviceNotReady() severity warning high format "TMP112 device not ready" throttle 5 + + @ Event for reporting TMP112 initialization failure + event DeviceInitFailed(ret: I32) severity warning high format "TMP112 initialization failed with return code: {}" throttle 5 + + @ Event for reporting TMP112 nil device error + event DeviceNil() severity warning high format "TMP112 device is nil" throttle 5 + + @ Event for reporting TMP112 nil state error + event DeviceStateNil() severity warning high format "TMP112 device state is nil" throttle 5 + + @ Event for reporting TCA unhealthy state + event TcaUnhealthy() severity warning high format "TMP112 TCA device is unhealthy" throttle 5 + + @ Event for reporting MUX unhealthy state + event MuxUnhealthy() severity warning high format "TMP112 MUX device is unhealthy" throttle 5 + + @ Event for reporting Load Switch not ready state + event LoadSwitchNotReady() severity warning high format "TMP112 Load Switch is not ready" throttle 5 + + @ Event for reporting TMP112 sensor fetch failure + event SensorSampleFetchFailed(ret: I32) severity warning high format "TMP112 sensor fetch failed with return code: {}" throttle 5 + + @ Event for reporting TMP112 sensor channel get failure + event SensorChannelGetFailed(ret: I32) severity warning high format "TMP112 sensor channel get failed with return code: {}" throttle 5 + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + @ 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 + + } +} diff --git a/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.hpp b/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.hpp new file mode 100644 index 00000000..cc45fc16 --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Tmp112Manager/TMP112Manager.hpp @@ -0,0 +1,98 @@ +// ====================================================================== +// \title Tmp112Manager.hpp +// \brief hpp file for Tmp112Manager component implementation class +// ====================================================================== + +#ifndef Drv_Tmp112Manager_HPP +#define Drv_Tmp112Manager_HPP + +#include "FprimeZephyrReference/Components/Drv/Tmp112Manager/Tmp112ManagerComponentAc.hpp" +#include +#include +#include +namespace Drv { + +class Tmp112Manager final : public Tmp112ManagerComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct Tmp112Manager object + Tmp112Manager(const char* const compName //!< The component name + ); + + //! Destroy Tmp112Manager object + ~Tmp112Manager(); + + public: + // ---------------------------------------------------------------------- + // Public helper methods + // ---------------------------------------------------------------------- + + //! Configure the TMP112 device + void configure(const struct device* tca, const struct device* mux, const struct device* dev, bool loadSwitchCheck); + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for loadSwitchStateChanged + //! + //! Port to initialize and deinitialize the TMP112 device on load switch state change + Fw::Success loadSwitchStateChanged_handler(FwIndexType portNum, //!< The port number + const Fw::On& state) override; + + //! Handler implementation for temperatureGet + //! + //! Port to read the temperature in degrees Celsius + F64 temperatureGet_handler(FwIndexType portNum, //!< The port number + Fw::Success& condition) override; + + private: + // ---------------------------------------------------------------------- + // Private helper methods + // ---------------------------------------------------------------------- + + //! Initialize the TMP112 device + Fw::Success initializeDevice(); + + //! Deinitialize the TMP112 device + Fw::Success deinitializeDevice(); + + //! Check if the TMP112 device is initialized + bool isDeviceInitialized(); + + //! Check if the load switch is ready (on and timeout passed) + bool loadSwitchReady(); + + private: + // ---------------------------------------------------------------------- + // Private member variables + // ---------------------------------------------------------------------- + + //! Zephyr device stores the initialized TMP112 sensor + const struct device* m_dev; + + //! Zephyr device for the TCA + const struct device* m_tca; + + //! Zephyr device for the mux + const struct device* m_mux; + + //! Load switch state + Fw::On m_load_switch_state = Fw::On::OFF; + + //! Load switch on timeout + //! Time when we can consider the load switch to be fully on (giving time for power to normalize) + Fw::Time m_load_switch_on_timeout; + + //! Load switch check + //! Available to disable if the component is not powered by a load switch + bool m_load_switch_check = true; +}; + +} // namespace Drv + +#endif diff --git a/FprimeZephyrReference/Components/Drv/Veml6031Manager/CMakeLists.txt b/FprimeZephyrReference/Components/Drv/Veml6031Manager/CMakeLists.txt new file mode 100644 index 00000000..b3fa8d69 --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Veml6031Manager/CMakeLists.txt @@ -0,0 +1,36 @@ +#### +# F Prime CMakeLists.txt: +# +# SOURCES: list of source files (to be compiled) +# AUTOCODER_INPUTS: list of files to be passed to the autocoders +# DEPENDS: list of libraries that this module depends on +# +# More information in the F´ CMake API documentation: +# https://fprime.jpl.nasa.gov/latest/docs/reference/api/cmake/API/ +# +#### + +# Module names are derived from the path from the nearest project/library/framework +# root when not specifically overridden by the developer. i.e. The module defined by +# `Ref/SignalGen/CMakeLists.txt` will be named `Ref_SignalGen`. + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/Veml6031Manager.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/Veml6031Manager.cpp" +# DEPENDS +# MyPackage_MyOtherModule +) + +### Unit Tests ### +# register_fprime_ut( +# AUTOCODER_INPUTS +# "${CMAKE_CURRENT_LIST_DIR}/Veml6031Manager.fpp" +# SOURCES +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Veml6031ManagerTestMain.cpp" +# "${CMAKE_CURRENT_LIST_DIR}/test/ut/Veml6031ManagerTester.cpp" +# DEPENDS +# STest # For rules-based testing +# UT_AUTO_HELPERS +# ) diff --git a/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.cpp b/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.cpp new file mode 100644 index 00000000..786b33ab --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.cpp @@ -0,0 +1,313 @@ +// ====================================================================== +// \title Veml6031Manager.cpp +// \brief cpp file for Veml6031Manager component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.hpp" + +#include +#include +#include +#include + +namespace Drv { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +Veml6031Manager ::Veml6031Manager(const char* const compName) : Veml6031ManagerComponentBase(compName) {} + +Veml6031Manager ::~Veml6031Manager() {} + +// ---------------------------------------------------------------------- +// Public helper methods +// ---------------------------------------------------------------------- + +void Veml6031Manager ::configure(const struct device* tca, const struct device* mux, const struct device* dev) { + this->m_tca = tca; + this->m_mux = mux; + this->m_dev = dev; +} + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +F32 Veml6031Manager ::ambientLightGet_handler(FwIndexType portNum, Fw::Success& condition) { + condition = Fw::Success::FAILURE; + + if (!this->initializeDevice()) { + return 0; + } + + configureSensorAttributes(SENSOR_CHAN_AMBIENT_LIGHT); // ignore return value for now + + int rc = sensor_sample_fetch_chan(this->m_dev, SENSOR_CHAN_AMBIENT_LIGHT); + if (rc != 0) { + this->log_WARNING_HI_SensorSampleFetchFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorSampleFetchFailed_ThrottleClear(); + + struct sensor_value val; + rc = sensor_channel_get(this->m_dev, SENSOR_CHAN_AMBIENT_LIGHT, &val); + if (rc != 0) { + this->log_WARNING_HI_SensorChannelGetFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorChannelGetFailed_ThrottleClear(); + + F32 lux = sensor_value_to_double(&val); + + this->tlmWrite_AmbientLight(lux); + + condition = Fw::Success::SUCCESS; + return lux; +} + +F32 Veml6031Manager ::infraRedLightGet_handler(FwIndexType portNum, Fw::Success& condition) { + condition = Fw::Success::FAILURE; + + if (!this->initializeDevice()) { + return 0; + } + + configureSensorAttributes(SENSOR_CHAN_IR); // ignore return value for now + + int rc = sensor_sample_fetch_chan(this->m_dev, SENSOR_CHAN_IR); + if (rc != 0) { + this->log_WARNING_HI_SensorSampleFetchFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorSampleFetchFailed_ThrottleClear(); + + struct sensor_value val; + rc = sensor_channel_get(this->m_dev, SENSOR_CHAN_IR, &val); + if (rc != 0) { + this->log_WARNING_HI_SensorChannelGetFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorChannelGetFailed_ThrottleClear(); + + F32 lux = sensor_value_to_double(&val); + + this->tlmWrite_InfraRedLight(lux); + + condition = Fw::Success::SUCCESS; + return lux; +} + +Fw::Success Veml6031Manager ::loadSwitchStateChanged_handler(FwIndexType portNum, const Fw::On& state) { + // Store the load switch state + this->m_load_switch_state = state; + + // If the load switch is off, deinitialize the device + if (this->m_load_switch_state == Fw::On::OFF) { + return this->deinitializeDevice(); + } + + // If the load switch is on, set the timeout + // We only consider the load switch to be fully on after a timeout period + this->m_load_switch_on_timeout = this->getTime(); + this->m_load_switch_on_timeout.add(1, 0); + + return Fw::Success::SUCCESS; +} + +F32 Veml6031Manager ::visibleLightGet_handler(FwIndexType portNum, Fw::Success& condition) { + condition = Fw::Success::FAILURE; + + if (!this->initializeDevice()) { + return 0; + } + + configureSensorAttributes(SENSOR_CHAN_LIGHT); // ignore return value for now + + int rc = sensor_sample_fetch_chan(this->m_dev, SENSOR_CHAN_LIGHT); + if (rc != 0) { + this->log_WARNING_HI_SensorSampleFetchFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorSampleFetchFailed_ThrottleClear(); + + struct sensor_value val; + rc = sensor_channel_get(this->m_dev, SENSOR_CHAN_LIGHT, &val); + if (rc != 0) { + this->log_WARNING_HI_SensorChannelGetFailed(rc); + return 0; + } + this->log_WARNING_HI_SensorChannelGetFailed_ThrottleClear(); + + F32 lux = sensor_value_to_double(&val); + + this->tlmWrite_VisibleLight(lux); + + condition = Fw::Success::SUCCESS; + return lux; +} + +// ---------------------------------------------------------------------- +// Private helper methods +// ---------------------------------------------------------------------- + +bool Veml6031Manager ::isDeviceInitialized() { + if (!this->m_dev) { + this->log_WARNING_HI_DeviceNil(); + return false; + } + this->log_WARNING_HI_DeviceNil_ThrottleClear(); + + if (!this->m_dev->state) { + this->log_WARNING_HI_DeviceStateNil(); + return false; + } + this->log_WARNING_HI_DeviceStateNil_ThrottleClear(); + + return this->m_dev->state->initialized; +} + +Fw::Success Veml6031Manager ::initializeDevice() { + if (this->isDeviceInitialized()) { + if (!device_is_ready(this->m_dev)) { + this->log_WARNING_HI_DeviceNotReady(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceNotReady_ThrottleClear(); + return Fw::Success::SUCCESS; + } + + if (!device_is_ready(this->m_tca)) { + this->log_WARNING_HI_TcaUnhealthy(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_TcaUnhealthy_ThrottleClear(); + + if (!device_is_ready(this->m_mux)) { + this->log_WARNING_HI_MuxUnhealthy(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_MuxUnhealthy_ThrottleClear(); + + if (!this->loadSwitchReady()) { + this->log_WARNING_HI_LoadSwitchNotReady(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_LoadSwitchNotReady_ThrottleClear(); + + int rc = device_init(this->m_dev); + if (rc < 0) { + this->log_WARNING_HI_DeviceInitFailed(rc); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceInitFailed_ThrottleClear(); + + return Fw::Success::SUCCESS; +} + +Fw::Success Veml6031Manager ::deinitializeDevice() { + if (!this->m_dev) { + this->log_WARNING_HI_DeviceNil(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceNil_ThrottleClear(); + + if (!this->m_dev->state) { + this->log_WARNING_HI_DeviceStateNil(); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_DeviceStateNil_ThrottleClear(); + + this->m_dev->state->initialized = false; + return Fw::Success::SUCCESS; +} + +bool Veml6031Manager ::loadSwitchReady() { + return this->m_load_switch_state == Fw::On::ON && this->getTime() >= this->m_load_switch_on_timeout; +} + +Fw::Success Veml6031Manager ::setIntegrationTimeAttribute(sensor_channel chan) { + Fw::ParamValid valid; + U8 it = this->paramGet_INTEGRATION_TIME(valid); + if (valid != Fw::ParamValid::VALID) { + this->log_WARNING_HI_InvalidIntegrationTimeParam(it); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_InvalidIntegrationTimeParam_ThrottleClear(); + + struct sensor_value attr; + sensor_value_from_double(&attr, it); + int rc = sensor_attr_set(this->m_dev, chan, (enum sensor_attribute)SENSOR_ATTR_VEML6031_IT, &attr); + if (rc) { + this->log_WARNING_HI_SensorAttrSetFailed(SENSOR_ATTR_VEML6031_IT, it, rc); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_SensorAttrSetFailed_ThrottleClear(); + + return Fw::Success::SUCCESS; +} + +Fw::Success Veml6031Manager ::setGainAttribute(sensor_channel chan) { + Fw::ParamValid valid; + U8 gain = this->paramGet_GAIN(valid); + if (valid != Fw::ParamValid::VALID) { + this->log_WARNING_HI_InvalidGainParam(gain); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_InvalidGainParam_ThrottleClear(); + + struct sensor_value attr; + sensor_value_from_double(&attr, gain); + int rc = sensor_attr_set(this->m_dev, chan, (enum sensor_attribute)SENSOR_ATTR_VEML6031_GAIN, &attr); + if (rc) { + this->log_WARNING_HI_SensorAttrSetFailed(SENSOR_ATTR_VEML6031_GAIN, gain, rc); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_SensorAttrSetFailed_ThrottleClear(); + + return Fw::Success::SUCCESS; +} + +Fw::Success Veml6031Manager ::setDiv4Attribute(sensor_channel chan) { + Fw::ParamValid valid; + U8 div4 = this->paramGet_EFFECTIVE_PHOTODIODE_SIZE(valid); + if (valid != Fw::ParamValid::VALID) { + this->log_WARNING_HI_InvalidDiv4Param(div4); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_InvalidDiv4Param_ThrottleClear(); + + struct sensor_value attr; + sensor_value_from_double(&attr, div4); + int rc = sensor_attr_set(this->m_dev, chan, (enum sensor_attribute)SENSOR_ATTR_VEML6031_DIV4, &attr); + if (rc) { + this->log_WARNING_HI_SensorAttrSetFailed(SENSOR_ATTR_VEML6031_DIV4, div4, rc); + return Fw::Success::FAILURE; + } + this->log_WARNING_HI_SensorAttrSetFailed_ThrottleClear(); + + return Fw::Success::SUCCESS; +} + +Fw::Success Veml6031Manager ::configureSensorAttributes(sensor_channel chan) { + Fw::Success result; + + result = this->setIntegrationTimeAttribute(chan); + if (result != Fw::Success::SUCCESS) { + return result; + } + + result = this->setGainAttribute(chan); + if (result != Fw::Success::SUCCESS) { + return result; + } + + result = this->setDiv4Attribute(chan); + if (result != Fw::Success::SUCCESS) { + return result; + } + + return Fw::Success::SUCCESS; +} + +} // namespace Drv diff --git a/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.fpp b/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.fpp new file mode 100644 index 00000000..22b36d52 --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.fpp @@ -0,0 +1,155 @@ +module Drv { + port lightGet(ref condition: Fw.Success) -> F32 + + @ Gain settings for VEML6031 sensor + enum GAIN : U8 { + VEML6031_GAIN_1 @< Gain of 1 + VEML6031_GAIN_2 @< Gain of 2 + VEML6031_GAIN_0_66 @< Gain of 0.66 + VEML6031_GAIN_0_5 @< Gain of 0.5 + } + + @ Integration time settings for VEML6031 sensor + enum IT : U8 { + VEML6031_IT_3_125 @< 3.25 ms integration time + VEML6031_IT_6_25 @< 6.25 ms integration time + VEML6031_IT_12_5 @< 12.5 ms integration time + VEML6031_IT_25 @< 25 ms integration time + VEML6031_IT_50 @< 50 ms integration time + VEML6031_IT_100 @< 100 ms integration time + VEML6031_IT_200 @< 200 ms integration time + VEML6031_IT_400 @< 400 ms integration time + VEML6031_IT_800 @< 800 ms integration time + } + + @ Effective photodiode size (DIV4) settings for VEML6031 sensor + enum DIV4 : U8 { + VEML6031_SIZE_4_4 + VEML6031_SIZE_1_4 + } + + @ Ambient Light Sensor (ALS) persistence protect number settings for VEML6031 sensor + enum ALS_PERS : U8 { + VEML60XX_PERS_1 @< Interrupt triggered every ALS reading + VEML60XX_PERS_2 @< Interrupt triggered after 2 consecutive ALS readings out of threshold + VEML60XX_PERS_4 @< Interrupt triggered after 4 consecutive ALS readings out of threshold + VEML60XX_PERS_8 @< Interrupt triggered after 8 consecutive ALS readings out of threshold + } +} + +module Drv { + passive component Veml6031Manager { + + #### Parameters #### + @ Parameter for setting the gain of the light senors + param GAIN: GAIN default GAIN.VEML6031_GAIN_0_5 + + @ Parameter for setting the integration time (IT) of the light sensors + param INTEGRATION_TIME: IT default IT.VEML6031_IT_25 + + @ Parameter for setting the effective photodiode size (DIV4) mode of the light sensors + param EFFECTIVE_PHOTODIODE_SIZE: DIV4 default DIV4.VEML6031_SIZE_1_4 + + @ Parameter for setting the Ambient Light Sensor (ALS) persistence protect number setting (PERS). + param ALS_PERSISTENCE_PROTECT_NUMBER: ALS_PERS default ALS_PERS.VEML60XX_PERS_1 + + #### Telemetry #### + @ Telemetry for the illuminance in the visible spectrum, in lux + telemetry VisibleLight: F32 + + @ Telemetry for the illuminance in the infra-red spectrum, in lux + telemetry InfraRedLight: F32 + + @ Telemetry for the ambient illuminance in visible spectrum, in lux + telemetry AmbientLight: F32 + + #### Ports #### + + @ Port to read the illuminance in visible spectrum, in lux + @ This channel represents the raw measurement counts provided by the sensor ALS register. + @ It is useful for estimating good values for integration time, effective photodiode size + @ and gain attributes in fetch and get mode. + sync input port visibleLightGet: lightGet + + @ Port to read the illuminance in infra-red spectrum, in lux + sync input port infraRedLightGet: lightGet + + @ Port to read the ambient illuminance in visible spectrum, in lux + sync input port ambientLightGet: lightGet + + @ Port to initialize and deinitialize the VEML6031 device on load switch state change + sync input port loadSwitchStateChanged: Components.loadSwitchStateChanged + + #### Events #### + + @ Event for reporting VEML6031 not ready error + event DeviceNotReady() severity warning high format "VEML6031 device not ready" throttle 5 + + @ Event for reporting VEML6031 initialization failure + event DeviceInitFailed(ret: I32) severity warning high format "VEML6031 initialization failed with return code: {}" throttle 5 + + @ Event for reporting VEML6031 nil device error + event DeviceNil() severity warning high format "VEML6031 device is nil" throttle 5 + + @ Event for reporting VEML6031 nil state error + event DeviceStateNil() severity warning high format "VEML6031 device state is nil" throttle 5 + + @ Event for reporting TCA unhealthy state + event TcaUnhealthy() severity warning high format "VEML6031 TCA device is unhealthy" throttle 5 + + @ Event for reporting MUX unhealthy state + event MuxUnhealthy() severity warning high format "VEML6031 MUX device is unhealthy" throttle 5 + + @ Event for reporting Load Switch not ready state + event LoadSwitchNotReady() severity warning high format "VEML6031 Load Switch is not ready" throttle 5 + + @ Event for reporting VEML6031 sensor fetch failure + event SensorSampleFetchFailed(ret: I32) severity warning high format "VEML6031 sensor fetch failed with return code: {}" throttle 5 + + @ Event for reporting VEML6031 sensor channel get failure + event SensorChannelGetFailed(ret: I32) severity warning high format "VEML6031 sensor channel get failed with return code: {}" throttle 5 + + @ Event for reporting invalid gain parameter + event InvalidGainParam(gain: U8) severity warning high format "VEML6031 invalid gain parameter: {}" throttle 5 + + @ Event for reporting invalid integration time parameter + event InvalidIntegrationTimeParam(it: U8) severity warning high format "VEML6031 invalid integration time parameter: {}" throttle 5 + + @ Event for reporting invalid effective photodiode size parameter + event InvalidDiv4Param(div4: U8) severity warning high format "VEML6031 invalid effective photodiode size parameter: {}" throttle 5 + + @ Event for reporting sensor attribute set failure + event SensorAttrSetFailed(attr: U16, val: U8, ret: I32) severity warning high format "VEML6031 sensor attribute {}={} set failed with return code: {}" throttle 5 + + ############################################################################### + # 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/Drv/Veml6031Manager/Veml6031Manager.hpp b/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.hpp new file mode 100644 index 00000000..f2552ede --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031Manager.hpp @@ -0,0 +1,128 @@ +// ====================================================================== +// \title Veml6031Manager.hpp +// \brief hpp file for Veml6031Manager component implementation class +// ====================================================================== + +#ifndef Components_Veml6031Manager_HPP +#define Components_Veml6031Manager_HPP + +#include + +#include "FprimeZephyrReference/Components/Drv/Veml6031Manager/Veml6031ManagerComponentAc.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace Drv { + +class Veml6031Manager final : public Veml6031ManagerComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct Veml6031Manager object + Veml6031Manager(const char* const compName //!< The component name + ); + + //! Destroy Veml6031Manager object + ~Veml6031Manager(); + + public: + // ---------------------------------------------------------------------- + // Public helper methods + // ---------------------------------------------------------------------- + + //! Configure the TMP112 device + void configure(const struct device* tca, const struct device* mux, const struct device* dev); + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for ambientLightGet + //! + //! Port to read the ambient illuminance in visible spectrum, in lux + F32 ambientLightGet_handler(FwIndexType portNum, //!< The port number + Fw::Success& condition) override; + + //! Handler implementation for infraRedLightGet + //! + //! Port to read the illuminance in infra-red spectrum, in lux + F32 infraRedLightGet_handler(FwIndexType portNum, //!< The port number + Fw::Success& condition) override; + + //! Handler implementation for loadSwitchStateChanged + //! + //! Port to initialize and deinitialize the VEML6031 device on load switch state change + Fw::Success loadSwitchStateChanged_handler(FwIndexType portNum, //!< The port number + const Fw::On& state) override; + + //! Handler implementation for visibleLightGet + //! + //! Port to read the illuminance in visible spectrum, in lux + //! This channel represents the raw measurement counts provided by the sensor ALS register. + //! It is useful for estimating good values for integration time, effective photodiode size + //! and gain attributes in fetch and get mode. + F32 visibleLightGet_handler(FwIndexType portNum, //!< The port number + Fw::Success& condition) override; + + private: + // ---------------------------------------------------------------------- + // Private helper methods + // ---------------------------------------------------------------------- + + //! Initialize the TMP112 device + Fw::Success initializeDevice(); + + //! Deinitialize the TMP112 device + Fw::Success deinitializeDevice(); + + //! Check if the TMP112 device is initialized + bool isDeviceInitialized(); + + //! Check if the load switch is ready (on and timeout passed) + bool loadSwitchReady(); + + //! Set the integration time attribute for the VEML6031 sensor + Fw::Success setIntegrationTimeAttribute(sensor_channel chan); + + //! Set the gain attribute for the VEML6031 sensor + Fw::Success setGainAttribute(sensor_channel chan); + + //! Set the div4 attribute for the VEML6031 sensor + Fw::Success setDiv4Attribute(sensor_channel chan); + + //! Set all sensor attributes for the VEML6031 sensor + Fw::Success configureSensorAttributes(sensor_channel chan); + + private: + // ---------------------------------------------------------------------- + // Private member variables + // ---------------------------------------------------------------------- + + //! Zephyr device stores the initialized TMP112 sensor + const struct device* m_dev; + + //! Zephyr device for the TCA + const struct device* m_tca; + + //! Zephyr device for the mux + const struct device* m_mux; + + //! Load switch state + Fw::On m_load_switch_state = Fw::On::OFF; + + //! Load switch on timeout + //! Time when we can consider the load switch to be fully on (giving time for power to normalize) + Fw::Time m_load_switch_on_timeout; +}; + +} // namespace Drv + +#endif diff --git a/FprimeZephyrReference/Components/Drv/Veml6031Manager/docs/sdd.md b/FprimeZephyrReference/Components/Drv/Veml6031Manager/docs/sdd.md new file mode 100644 index 00000000..f0672bcd --- /dev/null +++ b/FprimeZephyrReference/Components/Drv/Veml6031Manager/docs/sdd.md @@ -0,0 +1,66 @@ +# Components::Veml6031Manager + +Component for reading light sensor data + +## Usage Examples +Add usage examples here + +### Diagrams +Add diagrams here + +### Typical Usage +And the typical usage of the component here + +## Class Diagram +Add a class diagram here + +## Port Descriptions +| Name | Description | +|---|---| +|---|---| + +## Component States +Add component states in the chart below +| Name | Description | +|---|---| +|---|---| + +## Sequence Diagrams +Add sequence diagrams here + +## Parameters +| Name | Description | +|---|---| +|---|---| + +## Commands +| Name | Description | +|---|---| +|---|---| + +## Events +| Name | Description | +|---|---| +|---|---| + +## Telemetry +| Name | Description | +|---|---| +|---|---| + +## Unit Tests +Add unit test descriptions in the chart below +| Name | Description | Output | Coverage | +|---|---|---|---| +|---|---|---|---| + +## Requirements +Add requirements in the chart below +| Name | Description | Validation | +|---|---|---| +|---|---|---| + +## Change Log +| Date | Description | +|---|---| +|---| Initial Draft | diff --git a/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.cpp b/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.cpp index 002d8b6e..f16f6834 100644 --- a/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.cpp +++ b/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.cpp @@ -22,12 +22,6 @@ LoadSwitch ::~LoadSwitch() {} // Handler implementations for typed input ports // ---------------------------------------------------------------------- -void LoadSwitch ::Reset_handler(FwIndexType portNum) { - this->setLoadSwitchState(Fw::On::OFF); - k_sleep(K_MSEC(100)); - this->setLoadSwitchState(Fw::On::ON); -} - void LoadSwitch ::turnOn_handler(FwIndexType portNum) { this->setLoadSwitchState(Fw::On::ON); } @@ -55,10 +49,30 @@ void LoadSwitch ::TURN_OFF_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { // ---------------------------------------------------------------------- void LoadSwitch ::setLoadSwitchState(Fw::On state) { - Fw::Logic gpioValue = (state == Fw::On::ON) ? Fw::Logic::HIGH : Fw::Logic::LOW; + // Check if the state is changing + if (this->getLoadSwitchState() == state) { + return; + } + + // Set the load switch state + Fw::Logic gpioValue = state ? Fw::Logic::HIGH : Fw::Logic::LOW; this->gpioSet_out(0, gpioValue); + + // Inform downstream components of the state change + for (FwIndexType i = 0; i < this->getNum_loadSwitchStateChanged_OutputPorts(); i++) { + if (!this->isConnected_loadSwitchStateChanged_OutputPort(i)) { + continue; + } + this->loadSwitchStateChanged_out(i, state); + } this->log_ACTIVITY_HI_StatusChanged(state); this->tlmWrite_IsOn(state); } +Fw::On LoadSwitch ::getLoadSwitchState() { + Fw::Logic state; + this->gpioGet_out(0, state); + return state ? Fw::On::ON : Fw::On::OFF; +} + } // namespace Components diff --git a/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.fpp b/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.fpp index 33540ad0..c2f7339d 100644 --- a/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.fpp +++ b/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.fpp @@ -1,39 +1,32 @@ +module Components { + port loadSwitchStateGet -> Fw.On + port loadSwitchStateChanged($state: Fw.On) -> Fw.Success +} + module Components { @ A generic load switch for controlling power to components passive component LoadSwitch { - # One async command/port is required for active components - # This should be overridden by the developers with a useful command/port - - ############################################################################## - #### Uncomment the following examples to start customizing your component #### - ############################################################################## - - # @ Example async command - # async command COMMAND_NAME(param_name: U32) + @ Command to turn the load switch on sync command TURN_ON() + + @ Command to turn the load switch off sync command TURN_OFF() - # @ Example telemetry counter - # telemetry ExampleCounter: U64 + @ Telemetry channel for load switch state telemetry IsOn: Fw.On - # @ Example event - # event ExampleStateEvent(example_state: Fw.On) severity activity high id 0 format "State set to {}" + @ Event for reporting load switch state change event StatusChanged($state: Fw.On) severity activity high id 1 format "Load switch state changed to {}" - # @ Example port: receiving calls from the rate group - # sync input port run: Svc.Sched - #output port Status: Drv.GpioRead - #We will not be putting a Drv.GpioRead port here, we are using the Gpio Driver component which has this already! - @ Port sending calls to the GPIO driver output port gpioSet: Drv.GpioWrite + @ Port sending calls to the GPIO driver to read state + output port gpioGet: Drv.GpioRead - # Input that will be used by other components if they want to force a reset - # (off and on again) of the load switch - sync input port Reset: Fw.Signal + @ Port to indicate a change in load switch state + output port loadSwitchStateChanged: [2] loadSwitchStateChanged @ Input port to turn on the load switch (called by other components) sync input port turnOn: Fw.Signal @@ -41,9 +34,6 @@ module Components { @ Input port to turn off the load switch (called by other components) sync input port turnOff: Fw.Signal - # @ Example parameter - # param PARAMETER_NAME: U32 - ############################################################################### # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # ############################################################################### diff --git a/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.hpp b/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.hpp index 7e2862e2..8d89265d 100644 --- a/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.hpp +++ b/FprimeZephyrReference/Components/LoadSwitch/LoadSwitch.hpp @@ -43,14 +43,6 @@ class LoadSwitch final : public LoadSwitchComponentBase { U32 cmdSeq //!< The command sequence number ) override; - // ---------------------------------------------------------------------- - // Handler implementations for typed input ports - // ---------------------------------------------------------------------- - - //! Handler implementation for Reset - void Reset_handler(FwIndexType portNum //!< The port number - ) override; - //! Handler implementation for turnOn void turnOn_handler(FwIndexType portNum //!< The port number ) override; @@ -66,6 +58,9 @@ class LoadSwitch final : public LoadSwitchComponentBase { //! Set the load switch state (common implementation for commands and ports) void setLoadSwitchState(Fw::On state //!< The desired state (ON or OFF) ); + + //! Get current load switch state + Fw::On getLoadSwitchState(); //> + } + class LoadSwitch { + + LoadSwitch(const char* compName) + + ~LoadSwitch() + - TURN_ON_cmdHandler(FwOpcodeType opCode, U32 cmdSeq): void + - TURN_OFF_cmdHandler(FwOpcodeType opCode, U32 cmdSeq): void + - turnOn_handler(FwIndexType portNum): void + - turnOff_handler(FwIndexType portNum): void + } + } + LoadSwitchComponentBase <|-- LoadSwitch : inherits +``` ## External interface @@ -27,7 +45,7 @@ async `Reset` input which toggles the switch (off, short delay, on). | Name | Type | Description | |---|---:|---| -| IsOn | Fw.On | Current power state; written after commands and on Reset handling. | +| IsOn | Fw.On | Current power state; written after commands. Note: Reports ON only after a stabilization delay (default 1s) following the turn-on command. | ### Events @@ -35,15 +53,28 @@ async `Reset` input which toggles the switch (off, short delay, on). |---|---|---:|---| | StatusChanged | activity high | 1 | "Load switch state changed to {}" | -The component logs the `StatusChanged` event whenever the switch transitions due to a command or a Reset. +The component logs the `StatusChanged` event whenever the switch transitions due to a command. ### Ports | Port name | Direction | Port type | Notes | |---|---|---|---| | gpioSet | output | Drv.GpioWrite | Used to write the physical GPIO. Implementation always uses index 0 (`gpioSet_out(0, ...)`). | -| Reset | input (async) | Fw.Signal | Causes the component to perform a hardware reset sequence: LOW -> wait 100ms -> HIGH. | +| gpioGet | output | Drv.GpioRead | Used to read the physical GPIO state. | +| turnOn | input (sync) | Fw.Signal | Turns on the load switch. | +| turnOff | input (sync) | Fw.Signal | Turns off the load switch. | +| loadSwitchStateChanged | output | loadSwitchStateChanged | Notifies connected components when the load switch state changes | + +## Requirements +| Name | Description | Validation | +|---|---|---| +| Control via Command | The component shall allow turning the load switch on and off via ground commands `TURN_ON` and `TURN_OFF`. | Integration test | +| Control via Port | The component shall allow turning the load switch on and off via input ports `turnOn` and `turnOff`. | Verify `turnOn` and `turnOff` port calls change the GPIO state and telemetry. | +| State Telemetry | The component shall report the current state of the load switch via the `IsOn` telemetry channel. | Integration test | +| State Event | The component shall emit a `StatusChanged` event when the load switch state changes. | Verify `StatusChanged` event is emitted upon state transitions. | +| State Notification | The component shall notify connected components of state changes via the `loadSwitchStateChanged` port. | Downstream component testing | +| GPIO Control | The component shall control the physical GPIO pin corresponding to the load switch using the `gpioSet` port. | Downstream component testing | ## Change Log @@ -51,3 +82,4 @@ The component logs the `StatusChanged` event whenever the switch transitions due |---|---| | 10-22-2025 | Sarah, Kevin, and MoMata's first commit | | 11-07-2025 | Updated SDD to match implementation in `LoadSwitch.cpp/.hpp/.fpp` (commands, telemetry, event, ports, reset behavior). | +| 11-30-2025 | Removed Reset capability. Added `loadSwitchStateChanged` output port for state notifications. | diff --git a/FprimeZephyrReference/Components/ThermalManager/CMakeLists.txt b/FprimeZephyrReference/Components/ThermalManager/CMakeLists.txt new file mode 100644 index 00000000..16905ca2 --- /dev/null +++ b/FprimeZephyrReference/Components/ThermalManager/CMakeLists.txt @@ -0,0 +1,18 @@ +#### +# 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/ +# +#### + +register_fprime_library( + AUTOCODER_INPUTS + "${CMAKE_CURRENT_LIST_DIR}/ThermalManager.fpp" + SOURCES + "${CMAKE_CURRENT_LIST_DIR}/ThermalManager.cpp" +) diff --git a/FprimeZephyrReference/Components/ThermalManager/ThermalManager.cpp b/FprimeZephyrReference/Components/ThermalManager/ThermalManager.cpp new file mode 100644 index 00000000..17102ee2 --- /dev/null +++ b/FprimeZephyrReference/Components/ThermalManager/ThermalManager.cpp @@ -0,0 +1,38 @@ +// ====================================================================== +// \title ThermalManager.cpp +// \brief cpp file for ThermalManager component implementation class +// ====================================================================== + +#include "FprimeZephyrReference/Components/ThermalManager/ThermalManager.hpp" + +#include + +namespace Components { + +// ---------------------------------------------------------------------- +// Component construction and destruction +// ---------------------------------------------------------------------- + +ThermalManager::ThermalManager(const char* const compName) : ThermalManagerComponentBase(compName) {} + +ThermalManager::~ThermalManager() {} + +// ---------------------------------------------------------------------- +// Handler implementations for typed input ports +// ---------------------------------------------------------------------- + +void ThermalManager::run_handler(FwIndexType portNum, U32 context) { + Fw::Success condition; + + // Face temp sensors + for (FwIndexType i = 0; i < this->getNum_faceTempGet_OutputPorts(); i++) { + this->faceTempGet_out(i, condition); + } + + // Battery cell temp sensors + for (FwIndexType i = 0; i < this->getNum_battCellTempGet_OutputPorts(); i++) { + this->battCellTempGet_out(i, condition); + } +} + +} // namespace Components diff --git a/FprimeZephyrReference/Components/ThermalManager/ThermalManager.fpp b/FprimeZephyrReference/Components/ThermalManager/ThermalManager.fpp new file mode 100644 index 00000000..f3c73ab7 --- /dev/null +++ b/FprimeZephyrReference/Components/ThermalManager/ThermalManager.fpp @@ -0,0 +1,32 @@ +module Components { + @ Thermal Manager Component for F Prime FSW framework. + @ Orchestrates temperature sensor readings from 11 TMP112 sensors + passive component ThermalManager { + sync input port run: Svc.Sched + + # Output ports to instances + + @ Port for face temperature sensors + output port faceTempGet: [5] Drv.temperatureGet + + # Battery Cell + + @ Port for battery cell temperature sensors + output port battCellTempGet: [4] Drv.temperatureGet + + ############################################################################### + # Standard AC Ports: Required for Channels, Events, Commands, and Parameters # + ############################################################################### + @ Port for requesting the current time + time get port timeCaller + + @ Port for emitting telemetry + telemetry port tlmOut + + @ Port for emitting events + event port logOut + + @ Port for emitting text events + text event port logTextOut + } +} diff --git a/FprimeZephyrReference/Components/ThermalManager/ThermalManager.hpp b/FprimeZephyrReference/Components/ThermalManager/ThermalManager.hpp new file mode 100644 index 00000000..3bdf8989 --- /dev/null +++ b/FprimeZephyrReference/Components/ThermalManager/ThermalManager.hpp @@ -0,0 +1,41 @@ +// ====================================================================== +// \title ThermalManager.hpp +// \brief hpp file for ThermalManager component implementation class +// ====================================================================== + +#ifndef Components_ThermalManager_HPP +#define Components_ThermalManager_HPP + +#include "FprimeZephyrReference/Components/ThermalManager/ThermalManagerComponentAc.hpp" + +namespace Components { + +class ThermalManager final : public ThermalManagerComponentBase { + public: + // ---------------------------------------------------------------------- + // Component construction and destruction + // ---------------------------------------------------------------------- + + //! Construct ThermalManager object + ThermalManager(const char* const compName //!< The component name + ); + + //! Destroy ThermalManager object + ~ThermalManager(); + + private: + // ---------------------------------------------------------------------- + // Handler implementations for typed input ports + // ---------------------------------------------------------------------- + + //! Handler implementation for run + //! + //! Scheduled port for periodic temperature reading + void run_handler(FwIndexType portNum, //!< The port number + U32 context //!< The call order + ) override; +}; + +} // namespace Components + +#endif diff --git a/FprimeZephyrReference/ReferenceDeployment/Main.cpp b/FprimeZephyrReference/ReferenceDeployment/Main.cpp index c4d250c5..16b871ce 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Main.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Main.cpp @@ -10,6 +10,7 @@ #include #include +// Device bindings 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)); @@ -17,6 +18,31 @@ const struct device* lora = DEVICE_DT_GET(DT_NODELABEL(lora0)); 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)); +const struct device* tca9548a = DEVICE_DT_GET(DT_NODELABEL(tca9548a)); +const struct device* mux_channel_0 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_0)); +const struct device* mux_channel_1 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_1)); +const struct device* mux_channel_2 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_2)); +const struct device* mux_channel_3 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_3)); +const struct device* mux_channel_4 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_4)); +const struct device* mux_channel_5 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_5)); +const struct device* mux_channel_6 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_6)); +const struct device* mux_channel_7 = DEVICE_DT_GET(DT_NODELABEL(mux_channel_7)); +const struct device* face0_temp_sens = DEVICE_DT_GET(DT_NODELABEL(face0_temp_sens)); +const struct device* face1_temp_sens = DEVICE_DT_GET(DT_NODELABEL(face1_temp_sens)); +const struct device* face2_temp_sens = DEVICE_DT_GET(DT_NODELABEL(face2_temp_sens)); +const struct device* face3_temp_sens = DEVICE_DT_GET(DT_NODELABEL(face3_temp_sens)); +const struct device* face5_temp_sens = DEVICE_DT_GET(DT_NODELABEL(face5_temp_sens)); +const struct device* batt_cell1_temp_sens = DEVICE_DT_GET(DT_NODELABEL(batt_cell1_temp_sens)); +const struct device* batt_cell2_temp_sens = DEVICE_DT_GET(DT_NODELABEL(batt_cell2_temp_sens)); +const struct device* batt_cell3_temp_sens = DEVICE_DT_GET(DT_NODELABEL(batt_cell3_temp_sens)); +const struct device* batt_cell4_temp_sens = DEVICE_DT_GET(DT_NODELABEL(batt_cell4_temp_sens)); +const struct device* face0_light_sens = DEVICE_DT_GET(DT_NODELABEL(face0_light_sens)); +const struct device* face1_light_sens = DEVICE_DT_GET(DT_NODELABEL(face1_light_sens)); +const struct device* face2_light_sens = DEVICE_DT_GET(DT_NODELABEL(face2_light_sens)); +const struct device* face3_light_sens = DEVICE_DT_GET(DT_NODELABEL(face3_light_sens)); +const struct device* face5_light_sens = DEVICE_DT_GET(DT_NODELABEL(face5_light_sens)); +const struct device* face6_light_sens = DEVICE_DT_GET(DT_NODELABEL(face6_light_sens)); +const struct device* face7_light_sens = DEVICE_DT_GET(DT_NODELABEL(face7_light_sens)); int main(int argc, char* argv[]) { // ** DO NOT REMOVE **// @@ -25,8 +51,11 @@ int main(int argc, char* argv[]) { // the application starts writing to it. k_sleep(K_MSEC(3000)); Os::init(); + // Object for communicating state to the topology ReferenceDeployment::TopologyState inputs; + + // Flight Control Board device bindings inputs.ina219SysDevice = ina219Sys; inputs.ina219SolDevice = ina219Sol; inputs.loraDevice = lora; @@ -34,6 +63,36 @@ int main(int argc, char* argv[]) { inputs.lsm6dsoDevice = lsm6dso; inputs.lis2mdlDevice = lis2mdl; inputs.rtcDevice = rtc; + inputs.tca9548aDevice = tca9548a; + inputs.muxChannel0Device = mux_channel_0; + inputs.muxChannel1Device = mux_channel_1; + inputs.muxChannel2Device = mux_channel_2; + inputs.muxChannel3Device = mux_channel_3; + inputs.muxChannel4Device = mux_channel_4; + inputs.muxChannel5Device = mux_channel_5; + inputs.muxChannel6Device = mux_channel_6; + inputs.muxChannel7Device = mux_channel_7; + + // Face Board device bindings + // TMP112 temperature sensor devices + inputs.face0TempDevice = face0_temp_sens; + inputs.face1TempDevice = face1_temp_sens; + inputs.face2TempDevice = face2_temp_sens; + inputs.face3TempDevice = face3_temp_sens; + inputs.face5TempDevice = face5_temp_sens; + inputs.battCell1TempDevice = batt_cell1_temp_sens; + inputs.battCell2TempDevice = batt_cell2_temp_sens; + inputs.battCell3TempDevice = batt_cell3_temp_sens; + inputs.battCell4TempDevice = batt_cell4_temp_sens; + // Light sensor devices + inputs.face0LightDevice = face0_light_sens; + inputs.face1LightDevice = face1_light_sens; + inputs.face2LightDevice = face2_light_sens; + inputs.face3LightDevice = face3_light_sens; + inputs.face5LightDevice = face5_light_sens; + inputs.face6LightDevice = face6_light_sens; + inputs.face7LightDevice = face7_light_sens; + inputs.baudRate = 115200; // Setup, cycle, and teardown topology diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi index 6161868a..d3126da5 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentPackets.fppi @@ -9,6 +9,8 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.HiBuffs ReferenceDeployment.rateGroup10Hz.RgMaxTime ReferenceDeployment.rateGroup1Hz.RgMaxTime + ReferenceDeployment.rateGroup1_6Hz.RgMaxTime + ReferenceDeployment.rateGroup1_10Hz.RgMaxTime ReferenceDeployment.startupManager.BootCount ReferenceDeployment.startupManager.QuiescenceEndTime ReferenceDeployment.modeManager.CurrentMode @@ -23,6 +25,8 @@ telemetry packets ReferenceDeploymentPackets { ComCcsdsUart.commsBufferManager.EmptyBuffs ReferenceDeployment.rateGroup10Hz.RgCycleSlips ReferenceDeployment.rateGroup1Hz.RgCycleSlips + ReferenceDeployment.rateGroup1_6Hz.RgCycleSlips + ReferenceDeployment.rateGroup1_10Hz.RgCycleSlips } @@ -86,6 +90,28 @@ telemetry packets ReferenceDeploymentPackets { ReferenceDeployment.powerMonitor.TotalPowerGenerated } + packet Thermal id 11 group 4 { + ReferenceDeployment.tmp112Face0Manager.Temperature + ReferenceDeployment.tmp112Face1Manager.Temperature + ReferenceDeployment.tmp112Face2Manager.Temperature + ReferenceDeployment.tmp112Face3Manager.Temperature + ReferenceDeployment.tmp112Face5Manager.Temperature + ReferenceDeployment.tmp112BattCell1Manager.Temperature + ReferenceDeployment.tmp112BattCell2Manager.Temperature + ReferenceDeployment.tmp112BattCell3Manager.Temperature + ReferenceDeployment.tmp112BattCell4Manager.Temperature + } + + packet LightSensor id 13 group 4 { + ReferenceDeployment.veml6031Face0Manager.VisibleLight + ReferenceDeployment.veml6031Face1Manager.VisibleLight + ReferenceDeployment.veml6031Face2Manager.VisibleLight + ReferenceDeployment.veml6031Face3Manager.VisibleLight + ReferenceDeployment.veml6031Face5Manager.VisibleLight + ReferenceDeployment.veml6031Face6Manager.VisibleLight + ReferenceDeployment.veml6031Face7Manager.VisibleLight + } + } omit { CdhCore.cmdDisp.CommandErrors # Only has one library, no custom versions @@ -121,4 +147,18 @@ telemetry packets ReferenceDeploymentPackets { FileHandling.fileManager.Errors FileHandling.fileUplink.Warnings FileHandling.fileDownlink.Warnings + ReferenceDeployment.veml6031Face0Manager.InfraRedLight + ReferenceDeployment.veml6031Face1Manager.InfraRedLight + ReferenceDeployment.veml6031Face2Manager.InfraRedLight + ReferenceDeployment.veml6031Face3Manager.InfraRedLight + ReferenceDeployment.veml6031Face5Manager.InfraRedLight + ReferenceDeployment.veml6031Face6Manager.InfraRedLight + ReferenceDeployment.veml6031Face7Manager.InfraRedLight + ReferenceDeployment.veml6031Face0Manager.AmbientLight + ReferenceDeployment.veml6031Face1Manager.AmbientLight + ReferenceDeployment.veml6031Face2Manager.AmbientLight + ReferenceDeployment.veml6031Face3Manager.AmbientLight + ReferenceDeployment.veml6031Face5Manager.AmbientLight + ReferenceDeployment.veml6031Face6Manager.AmbientLight + ReferenceDeployment.veml6031Face7Manager.AmbientLight } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp index af8e02a4..e8ae548e 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopology.cpp @@ -42,14 +42,18 @@ constexpr FwSizeType getRateGroupPeriod(const FwSizeType hz) { // The reference topology divides the incoming clock signal (1Hz) into sub-signals: 1Hz, 1/2Hz, and 1/4Hz with 0 offset Svc::RateGroupDriver::DividerSet rateGroupDivisorsSet{{ // Array of divider objects - {getRateGroupPeriod(10), 0}, // 10Hz - {getRateGroupPeriod(1), 0}, // 1Hz + {getRateGroupPeriod(10), 0}, // 10Hz = 100ms + {getRateGroupPeriod(1), 0}, // 1Hz = 1s + {6000, 0}, // 1/6Hz = 6s + {12000, 0} // 1/10Hz = 10s }}; // Rate groups may supply a context token to each of the attached children whose purpose is set by the project. The // reference topology sets each token to zero as these contexts are unused in this project. U32 rateGroup10HzContext[Svc::ActiveRateGroup::CONNECTION_COUNT_MAX] = {getRateGroupPeriod(10)}; U32 rateGroup1HzContext[Svc::ActiveRateGroup::CONNECTION_COUNT_MAX] = {getRateGroupPeriod(1)}; +U32 rateGroup1_6HzContext[Svc::ActiveRateGroup::CONNECTION_COUNT_MAX] = {6000}; +U32 rateGroup1_10HzContext[Svc::ActiveRateGroup::CONNECTION_COUNT_MAX] = {12000}; /** * \brief configure/setup components in project-specific way @@ -65,6 +69,8 @@ void configureTopology() { // Rate groups require context arrays. rateGroup10Hz.configure(rateGroup10HzContext, FW_NUM_ARRAY_ELEMENTS(rateGroup10HzContext)); rateGroup1Hz.configure(rateGroup1HzContext, FW_NUM_ARRAY_ELEMENTS(rateGroup1HzContext)); + rateGroup1_6Hz.configure(rateGroup1_6HzContext, FW_NUM_ARRAY_ELEMENTS(rateGroup1_6HzContext)); + rateGroup1_10Hz.configure(rateGroup1_10HzContext, FW_NUM_ARRAY_ELEMENTS(rateGroup1_10HzContext)); gpioWatchdog.open(ledGpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); gpioBurnwire0.open(burnwire0Gpio, Zephyr::ZephyrGpioDriver::GpioConfiguration::OUT); @@ -115,6 +121,26 @@ void setupTopology(const TopologyState& state) { lis2mdlManager.configure(state.lis2mdlDevice); ina219SysManager.configure(state.ina219SysDevice); ina219SolManager.configure(state.ina219SolDevice); + + // Configure TMP112 temperature sensor managers + tmp112Face0Manager.configure(state.tca9548aDevice, state.muxChannel0Device, state.face0TempDevice, true); + tmp112Face1Manager.configure(state.tca9548aDevice, state.muxChannel1Device, state.face1TempDevice, true); + tmp112Face2Manager.configure(state.tca9548aDevice, state.muxChannel2Device, state.face2TempDevice, true); + tmp112Face3Manager.configure(state.tca9548aDevice, state.muxChannel3Device, state.face3TempDevice, true); + tmp112Face5Manager.configure(state.tca9548aDevice, state.muxChannel5Device, state.face5TempDevice, true); + tmp112BattCell1Manager.configure(state.tca9548aDevice, state.muxChannel4Device, state.battCell1TempDevice, false); + tmp112BattCell2Manager.configure(state.tca9548aDevice, state.muxChannel4Device, state.battCell2TempDevice, false); + tmp112BattCell3Manager.configure(state.tca9548aDevice, state.muxChannel4Device, state.battCell3TempDevice, false); + tmp112BattCell4Manager.configure(state.tca9548aDevice, state.muxChannel4Device, state.battCell4TempDevice, false); + + // Configure VEML6031 light sensor managers + veml6031Face0Manager.configure(state.tca9548aDevice, state.muxChannel0Device, state.face0LightDevice); + veml6031Face1Manager.configure(state.tca9548aDevice, state.muxChannel1Device, state.face1LightDevice); + veml6031Face2Manager.configure(state.tca9548aDevice, state.muxChannel2Device, state.face2LightDevice); + veml6031Face3Manager.configure(state.tca9548aDevice, state.muxChannel3Device, state.face3LightDevice); + veml6031Face5Manager.configure(state.tca9548aDevice, state.muxChannel5Device, state.face5LightDevice); + veml6031Face6Manager.configure(state.tca9548aDevice, state.muxChannel6Device, state.face6LightDevice); + veml6031Face7Manager.configure(state.tca9548aDevice, state.muxChannel7Device, state.face7LightDevice); } void startRateGroups() { diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp index 00b11005..efeb40eb 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/ReferenceDeploymentTopologyDefs.hpp @@ -53,6 +53,12 @@ enum { WARN = 3, FATAL = 5 }; namespace ReferenceDeployment_rateGroup1Hz { enum { WARN = 3, FATAL = 5 }; } +namespace ReferenceDeployment_rateGroup1_6Hz { +enum { WARN = 3, FATAL = 5 }; +} +namespace ReferenceDeployment_rateGroup1_10Hz { +enum { WARN = 3, FATAL = 5 }; +} namespace ReferenceDeployment_cmdSeq { enum { WARN = 3, FATAL = 5 }; } @@ -80,6 +86,35 @@ struct TopologyState { 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* tca9548aDevice; //!< TCA9548A I2C multiplexer device + const device* muxChannel0Device; //!< Multiplexer channel 0 device + const device* muxChannel1Device; //!< Multiplexer channel 1 device + const device* muxChannel2Device; //!< Multiplexer channel 2 device + const device* muxChannel3Device; //!< Multiplexer channel 3 device + const device* muxChannel4Device; //!< Multiplexer channel 4 device + const device* muxChannel5Device; //!< Multiplexer channel 5 device + const device* muxChannel6Device; //!< Multiplexer channel 5 device + const device* muxChannel7Device; //!< Multiplexer channel 7 device + + // Face devices + // - Temperature sensors + const device* face0TempDevice; //!< TMP112 device for cube face 0 + const device* face1TempDevice; //!< TMP112 device for cube face 1 + const device* face2TempDevice; //!< TMP112 device for cube face 2 + const device* face3TempDevice; //!< TMP112 device for cube face 3 + const device* face5TempDevice; //!< TMP112 device for cube face 5 + const device* battCell1TempDevice; //!< TMP112 device for battery cell 1 + const device* battCell2TempDevice; //!< TMP112 device for battery cell 2 + const device* battCell3TempDevice; //!< TMP112 device for battery cell 3 + const device* battCell4TempDevice; //!< TMP112 device for battery cell 4 + // - Light sensors + const device* face0LightDevice; //!< Light sensor device for cube face 0 + const device* face1LightDevice; //!< Light sensor device for cube face 1 + const device* face2LightDevice; //!< Light sensor device for cube face 2 + const device* face3LightDevice; //!< Light sensor device for cube face 3 + const device* face5LightDevice; //!< Light sensor device for cube face 5 + const device* face6LightDevice; //!< Light sensor device for cube face 6 + const device* face7LightDevice; //!< Light sensor device for cube face 7 }; namespace PingEntries = ::PingEntries; diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp index 42bae188..2f55d3ce 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/instances.fpp @@ -37,6 +37,16 @@ module ReferenceDeployment { stack size Default.STACK_SIZE \ priority 3 + instance rateGroup1_6Hz: Svc.ActiveRateGroup base id 0x10003000 \ + queue size Default.QUEUE_SIZE \ + stack size Default.STACK_SIZE \ + priority 4 + + instance rateGroup1_10Hz: Svc.ActiveRateGroup base id 0x10004000 \ + queue size Default.QUEUE_SIZE \ + stack size Default.STACK_SIZE \ + priority 5 + instance cmdSeq: Svc.CmdSequencer base id 0x10006000 \ queue size Default.QUEUE_SIZE * 2 \ stack size Default.STACK_SIZE \ @@ -142,4 +152,28 @@ module ReferenceDeployment { instance startupManager: Components.StartupManager base id 0x1003F000 + # Thermal Management System + instance thermalManager: Components.ThermalManager base id 0x10041000 + instance tmp112Face0Manager: Drv.Tmp112Manager base id 0x10042000 + instance tmp112Face1Manager: Drv.Tmp112Manager base id 0x10043000 + instance tmp112Face2Manager: Drv.Tmp112Manager base id 0x10044000 + instance tmp112Face3Manager: Drv.Tmp112Manager base id 0x10045000 + instance tmp112Face4Manager: Drv.Tmp112Manager base id 0x10046000 + instance tmp112Face5Manager: Drv.Tmp112Manager base id 0x10047000 + instance tmp112BattCell1Manager: Drv.Tmp112Manager base id 0x10048000 + instance tmp112BattCell2Manager: Drv.Tmp112Manager base id 0x10049000 + instance tmp112BattCell3Manager: Drv.Tmp112Manager base id 0x1004A000 + instance tmp112BattCell4Manager: Drv.Tmp112Manager base id 0x1004B000 + + # Attitude Determination and Control System (ADCS) + instance adcs: Components.ADCS base id 0x1004C000 + instance veml6031Face0Manager: Drv.Veml6031Manager base id 0x1004D000 + instance veml6031Face1Manager: Drv.Veml6031Manager base id 0x1004E000 + instance veml6031Face2Manager: Drv.Veml6031Manager base id 0x1004F000 + instance veml6031Face3Manager: Drv.Veml6031Manager base id 0x10050000 + instance veml6031Face4Manager: Drv.Veml6031Manager base id 0x10051000 + instance veml6031Face5Manager: Drv.Veml6031Manager base id 0x10052000 + instance veml6031Face6Manager: Drv.Veml6031Manager base id 0x10053000 + instance veml6031Face7Manager: Drv.Veml6031Manager base id 0x10054000 + } diff --git a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp index 0390534e..7f467bcd 100644 --- a/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp +++ b/FprimeZephyrReference/ReferenceDeployment/Top/topology.fpp @@ -7,6 +7,8 @@ module ReferenceDeployment { enum Ports_RateGroups { rateGroup10Hz rateGroup1Hz + rateGroup1_6Hz + rateGroup1_10Hz } topology ReferenceDeployment { @@ -24,6 +26,8 @@ module ReferenceDeployment { # ---------------------------------------------------------------------- instance rateGroup10Hz instance rateGroup1Hz + instance rateGroup1_6Hz + instance rateGroup1_10Hz instance rateGroupDriver instance timer instance lora @@ -66,8 +70,28 @@ module ReferenceDeployment { instance powerMonitor instance ina219SysManager instance ina219SolManager + + # Face Board Instances + instance thermalManager + instance tmp112Face0Manager + instance tmp112Face1Manager + instance tmp112Face2Manager + instance tmp112Face3Manager + instance tmp112Face5Manager + instance tmp112BattCell1Manager + instance tmp112BattCell2Manager + instance tmp112BattCell3Manager + instance tmp112BattCell4Manager instance resetManager instance modeManager + instance adcs + instance veml6031Face0Manager + instance veml6031Face1Manager + instance veml6031Face2Manager + instance veml6031Face3Manager + instance veml6031Face5Manager + instance veml6031Face6Manager + instance veml6031Face7Manager # ---------------------------------------------------------------------- @@ -164,16 +188,21 @@ module ReferenceDeployment { rateGroup1Hz.RateGroupMemberOut[2] -> ComCcsds.commsBufferManager.schedIn rateGroup1Hz.RateGroupMemberOut[3] -> CdhCore.tlmSend.Run rateGroup1Hz.RateGroupMemberOut[4] -> watchdog.run - rateGroup1Hz.RateGroupMemberOut[5] -> imuManager.run - rateGroup1Hz.RateGroupMemberOut[6] -> comDelay.run - rateGroup1Hz.RateGroupMemberOut[7] -> burnwire.schedIn - rateGroup1Hz.RateGroupMemberOut[8] -> antennaDeployer.schedIn - rateGroup1Hz.RateGroupMemberOut[9] -> fsSpace.run - rateGroup1Hz.RateGroupMemberOut[10] -> FileHandling.fileDownlink.Run - rateGroup1Hz.RateGroupMemberOut[11] -> startupManager.run - rateGroup1Hz.RateGroupMemberOut[12] -> powerMonitor.run - rateGroup1Hz.RateGroupMemberOut[13] -> modeManager.run - + rateGroup1Hz.RateGroupMemberOut[5] -> comDelay.run + rateGroup1Hz.RateGroupMemberOut[6] -> burnwire.schedIn + rateGroup1Hz.RateGroupMemberOut[7] -> antennaDeployer.schedIn + rateGroup1Hz.RateGroupMemberOut[8] -> fsSpace.run + rateGroup1Hz.RateGroupMemberOut[9] -> FileHandling.fileDownlink.Run + rateGroup1Hz.RateGroupMemberOut[10] -> startupManager.run # doubles (20ms) rate group max time + rateGroup1Hz.RateGroupMemberOut[11] -> modeManager.run + + rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1_6Hz] -> rateGroup1_6Hz.CycleIn + rateGroup1_6Hz.RateGroupMemberOut[0] -> powerMonitor.run + rateGroup1_6Hz.RateGroupMemberOut[1] -> imuManager.run + rateGroup1_6Hz.RateGroupMemberOut[2] -> adcs.run + rateGroup1_6Hz.RateGroupMemberOut[3] -> thermalManager.run + + rateGroupDriver.CycleOut[Ports_RateGroups.rateGroup1_10Hz] -> rateGroup1_10Hz.CycleIn } @@ -183,13 +212,38 @@ module ReferenceDeployment { connections LoadSwitches { face4LoadSwitch.gpioSet -> gpioface4LS.gpioWrite + face4LoadSwitch.gpioGet -> gpioface4LS.gpioRead + face0LoadSwitch.gpioSet -> gpioface0LS.gpioWrite + face0LoadSwitch.gpioGet -> gpioface0LS.gpioRead + face0LoadSwitch.loadSwitchStateChanged[0] -> tmp112Face0Manager.loadSwitchStateChanged + face0LoadSwitch.loadSwitchStateChanged[1] -> veml6031Face0Manager.loadSwitchStateChanged + face1LoadSwitch.gpioSet -> gpioface1LS.gpioWrite + face1LoadSwitch.gpioGet -> gpioface1LS.gpioRead + face1LoadSwitch.loadSwitchStateChanged[0] -> tmp112Face1Manager.loadSwitchStateChanged + face1LoadSwitch.loadSwitchStateChanged[1] -> veml6031Face1Manager.loadSwitchStateChanged + face2LoadSwitch.gpioSet -> gpioface2LS.gpioWrite + face2LoadSwitch.gpioGet -> gpioface2LS.gpioRead + face2LoadSwitch.loadSwitchStateChanged[0] -> tmp112Face2Manager.loadSwitchStateChanged + face2LoadSwitch.loadSwitchStateChanged[1] -> veml6031Face2Manager.loadSwitchStateChanged + face3LoadSwitch.gpioSet -> gpioface3LS.gpioWrite + face3LoadSwitch.gpioGet -> gpioface3LS.gpioRead + face3LoadSwitch.loadSwitchStateChanged[0] -> tmp112Face3Manager.loadSwitchStateChanged + face3LoadSwitch.loadSwitchStateChanged[1] -> veml6031Face3Manager.loadSwitchStateChanged + face5LoadSwitch.gpioSet -> gpioface5LS.gpioWrite + face5LoadSwitch.gpioGet -> gpioface5LS.gpioRead + face5LoadSwitch.loadSwitchStateChanged[0] -> tmp112Face5Manager.loadSwitchStateChanged + face5LoadSwitch.loadSwitchStateChanged[1] -> veml6031Face5Manager.loadSwitchStateChanged + payloadPowerLoadSwitch.gpioSet -> gpioPayloadPowerLS.gpioWrite + payloadPowerLoadSwitch.gpioGet -> gpioPayloadPowerLS.gpioRead + payloadBatteryLoadSwitch.gpioSet -> gpioPayloadBatteryLS.gpioWrite + payloadBatteryLoadSwitch.gpioGet -> gpioPayloadBatteryLS.gpioRead } connections BurnwireGpio { @@ -229,6 +283,44 @@ module ReferenceDeployment { powerMonitor.solPowerGet -> ina219SolManager.powerGet } + connections thermalManager { + thermalManager.faceTempGet[0] -> tmp112Face0Manager.temperatureGet + thermalManager.faceTempGet[1] -> tmp112Face1Manager.temperatureGet + thermalManager.faceTempGet[2] -> tmp112Face2Manager.temperatureGet + thermalManager.faceTempGet[3] -> tmp112Face3Manager.temperatureGet + thermalManager.faceTempGet[4] -> tmp112Face5Manager.temperatureGet + thermalManager.battCellTempGet[0] -> tmp112BattCell1Manager.temperatureGet + thermalManager.battCellTempGet[1] -> tmp112BattCell2Manager.temperatureGet + thermalManager.battCellTempGet[2] -> tmp112BattCell3Manager.temperatureGet + thermalManager.battCellTempGet[3] -> tmp112BattCell4Manager.temperatureGet + } + + connections adcs { + adcs.visibleLightGet[0] -> veml6031Face0Manager.visibleLightGet + adcs.infraRedLightGet[0] -> veml6031Face0Manager.infraRedLightGet + adcs.ambientLightGet[0] -> veml6031Face0Manager.ambientLightGet + + adcs.visibleLightGet[1] -> veml6031Face1Manager.visibleLightGet + adcs.infraRedLightGet[1] -> veml6031Face1Manager.infraRedLightGet + adcs.ambientLightGet[1] -> veml6031Face1Manager.ambientLightGet + + adcs.visibleLightGet[2] -> veml6031Face2Manager.visibleLightGet + adcs.infraRedLightGet[2] -> veml6031Face2Manager.infraRedLightGet + adcs.ambientLightGet[2] -> veml6031Face2Manager.ambientLightGet + + adcs.visibleLightGet[3] -> veml6031Face3Manager.visibleLightGet + adcs.infraRedLightGet[3] -> veml6031Face3Manager.infraRedLightGet + adcs.ambientLightGet[3] -> veml6031Face3Manager.ambientLightGet + + adcs.visibleLightGet[4] -> veml6031Face5Manager.visibleLightGet + adcs.infraRedLightGet[4] -> veml6031Face5Manager.infraRedLightGet + adcs.ambientLightGet[4] -> veml6031Face5Manager.ambientLightGet + + adcs.visibleLightGet[5] -> veml6031Face6Manager.visibleLightGet + adcs.infraRedLightGet[5] -> veml6031Face6Manager.infraRedLightGet + adcs.ambientLightGet[5] -> veml6031Face6Manager.ambientLightGet + } + connections ModeManager { # Voltage monitoring from system power manager modeManager.voltageGet -> ina219SysManager.voltageGet diff --git a/FprimeZephyrReference/project/config/AcConstants.fpp b/FprimeZephyrReference/project/config/AcConstants.fpp index 8eeefb4b..2408d32a 100644 --- a/FprimeZephyrReference/project/config/AcConstants.fpp +++ b/FprimeZephyrReference/project/config/AcConstants.fpp @@ -4,16 +4,16 @@ # ====================================================================== @ Number of rate group member output ports for ActiveRateGroup -constant ActiveRateGroupOutputPorts = 15 +constant ActiveRateGroupOutputPorts = 16 @ Number of rate group member output ports for PassiveRateGroup constant PassiveRateGroupOutputPorts = 10 @ Used to drive rate groups -constant RateGroupDriverRateGroupPorts = 3 +constant RateGroupDriverRateGroupPorts = 4 @ 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/CommandDispatcherImplCfg.hpp b/FprimeZephyrReference/project/config/CommandDispatcherImplCfg.hpp index e4bd50b9..6f0a521a 100644 --- a/FprimeZephyrReference/project/config/CommandDispatcherImplCfg.hpp +++ b/FprimeZephyrReference/project/config/CommandDispatcherImplCfg.hpp @@ -11,7 +11,7 @@ // Define configuration values for dispatcher enum { - CMD_DISPATCHER_DISPATCH_TABLE_SIZE = 128, // !< The size of the table holding opcodes to dispatch + CMD_DISPATCHER_DISPATCH_TABLE_SIZE = 192, // !< The size of the table holding opcodes to dispatch CMD_DISPATCHER_SEQUENCER_TABLE_SIZE = 10, // !< The size of the table holding commands in progress }; diff --git a/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp b/FprimeZephyrReference/project/config/TlmPacketizerCfg.hpp index 635eaa3e..16071fc2 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 = 13; static const FwChanIdType TLMPACKETIZER_NUM_TLM_HASH_SLOTS = 15; // !< Number of slots in the hash table. // Works best when set to about twice the number of components producing telemetry @@ -24,13 +24,9 @@ static const FwChanIdType TLMPACKETIZER_HASH_MOD_VALUE = 999; // !< The modulo value of the hashing function. // Should be set to a little below the ID gaps to spread the entries around -// Increased from 90 to 512 buckets to accommodate anticipated future growth in telemetry channels, -// and to minimize hash collisions for improved performance. While currently only a few new channels -// are added (e.g., ModeManager: CurrentMode, SafeModeEntryCount), this sizing is -// intentionally over-provisioned to support planned expansion and to ensure efficient hash table usage. static const FwChanIdType TLMPACKETIZER_HASH_BUCKETS = - 512; // !< Buckets assignable to a hash slot. - // Buckets must be >= number of telemetry channels in system + 130; // !< Buckets assignable to a hash slot. + // Buckets must be >= number of telemetry channels + collision overhead static const FwChanIdType TLMPACKETIZER_MAX_MISSING_TLM_CHECK = 25; // !< Maximum number of missing telemetry channel checks diff --git a/prj.conf b/prj.conf index 743d7d1f..dba078a8 100644 --- a/prj.conf +++ b/prj.conf @@ -39,7 +39,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=30 +CONFIG_DYNAMIC_THREAD_POOL_SIZE=25 # Num threads in the thread pool CONFIG_DYNAMIC_THREAD_STACK_SIZE=4096 # Size of thread stack in thread pool, must be >= Thread Pool size in F' @@ -54,7 +54,7 @@ CONFIG_COMMON_LIBC_MALLOC=y CONFIG_SENSOR=y # Enable detailed logging for I2C and sensor debugging -CONFIG_LOG=y +CONFIG_LOG=n CONFIG_LOG_DEFAULT_LEVEL=3 CONFIG_CBPRINTF_FP_SUPPORT=y