(WIP): Magnetorquer Manager

FprimeZephyrReference/Components/Drv/CMakeLists.txt

@@ -3,3 +3,4 @@ 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}/Types/")
+add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/MagnetorquerManager/")

FprimeZephyrReference/Components/Drv/MagnetorquerManager/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}/MagnetorquerManager.fpp"
+    SOURCES
+        "${CMAKE_CURRENT_LIST_DIR}/MagnetorquerManager.cpp"
+#   DEPENDS
+#       MyPackage_MyOtherModule
+)
+
+### Unit Tests ###
+# register_fprime_ut(
+#     AUTOCODER_INPUTS
+#         "${CMAKE_CURRENT_LIST_DIR}/MagnetorquerManager.fpp"
+#     SOURCES
+#         "${CMAKE_CURRENT_LIST_DIR}/test/ut/MagnetorquerManagerTestMain.cpp"
+#         "${CMAKE_CURRENT_LIST_DIR}/test/ut/MagnetorquerManagerTester.cpp"
+#     DEPENDS
+#         STest # For rules-based testing
+#     UT_AUTO_HELPERS
+# )

FprimeZephyrReference/Components/Drv/MagnetorquerManager/MagnetorquerManager.cpp

@@ -0,0 +1,71 @@
+// ======================================================================
+// \title  MagnetorquerManager.cpp
+// \author aychar
+// \brief  cpp file for MagnetorquerManager component implementation class
+// ======================================================================
+
+#include "FprimeZephyrReference/Components/Drv/MagnetorquerManager/MagnetorquerManager.hpp"
+
+#include <zephyr/drivers/haptics/drv2605.h>
+#include <zephyr/kernel.h>
+
+namespace Drv {
+
+// ----------------------------------------------------------------------
+// Component construction and destruction
+// ----------------------------------------------------------------------
+
+MagnetorquerManager ::MagnetorquerManager(const char* const compName) : MagnetorquerManagerComponentBase(compName) {}
+
+MagnetorquerManager ::~MagnetorquerManager() {}
+
+// ----------------------------------------------------------------------
+// Helper methods
+// ----------------------------------------------------------------------
+
+void MagnetorquerManager ::configure(const struct device* const devices[5]) {
+    for (int i = 0; i < 5; ++i) {
+        this->m_devices[i] = devices[i];
+    }
+}
+
+void MagnetorquerManager ::START_PLAYBACK_TEST_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, U8 faceIdx) {
+    // Validate face index (0..4)
+    if (faceIdx >= 5) {
+        this->log_WARNING_LO_InvalidFaceIndex();
+        return;
+    }
+
+    const struct device* dev = this->m_devices[faceIdx];
+    if (!device_is_ready(dev)) {
+        this->log_WARNING_HI_DeviceNotReady();
+        return;
+    }
+
+    drv2605_haptic_config(dev, DRV2605_HAPTICS_SOURCE_ROM, (union drv2605_config_data*)&this->rom);
+}
+
+void MagnetorquerManager ::START_PLAYBACK_TEST2_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, U8 faceIdx) {
+    // Validate face index (0..4)
+    if (faceIdx >= 5) {
+        this->log_WARNING_LO_InvalidFaceIndex();
+        return;
+    }
+
+    const struct device* dev = this->m_devices[faceIdx];
+    if (!device_is_ready(dev)) {
+        this->log_WARNING_HI_DeviceNotReady();
+        return;
+    }
+
+    struct drv2605_rom_data rom2 = {.library = DRV2605_LIBRARY_TS2200_A, .seq_regs = {50, 0, 0, 0, 0, 0, 0, 0}};
+    drv2605_haptic_config(dev, DRV2605_HAPTICS_SOURCE_ROM, (union drv2605_config_data*)&rom2);
+}
+
+void MagnetorquerManager ::SetMagnetorquers_handler(const FwIndexType portNum, const Drv::InputArray& value) {
+    // TODO(hrfarmer): Once its possible to properly interact with the DRV2605, I'll figure out how to
+    // determine how the passed in amps should translate to a specific pattern(s) that should be ran.
+    return;
+}
+
+}  // namespace Drv

FprimeZephyrReference/Components/Drv/MagnetorquerManager/MagnetorquerManager.fpp

@@ -0,0 +1,60 @@
+module Drv {
+    array InputArray = [5] I32;
+    port SetMagnetorquers(
+        value: InputArray @< Amp value for each face in the order x1, x2, y1, y2, z1
+    )
+}
+
+module Drv {
+    @ Component for F Prime FSW framework.
+    passive component MagnetorquerManager {
+
+        @ Event for reporting DRV2605 not ready error
+        event DeviceNotReady() severity warning high format "DRV2605 device not ready" throttle 5
+
+        @ Event to report an invalid face index passed in
+        event InvalidFaceIndex() severity warning low format "The faceIdx should be between 0-4"
+
+        @ Start DRV2605 playback on a device with effect #47 on a specific face
+        @ faceIdx: index of the face to actuate (valid range: 0..4)
+        sync command START_PLAYBACK_TEST(faceIdx: U8)
+
+        @ Start DRV2605 playback on a device with effect #50 on a specific face
+        @ faceIdx: index of the face to actuate (valid range: 0..4)
+        sync command START_PLAYBACK_TEST2(faceIdx: U8)
+
+        @ Input port to set magnetorquer values
+        sync input port SetMagnetorquers: SetMagnetorquers
+
+        ###############################################################################
+        # 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
+
+    }
+}

FprimeZephyrReference/Components/Drv/MagnetorquerManager/MagnetorquerManager.hpp

@@ -0,0 +1,50 @@
+// ======================================================================
+// \title  MagnetorquerManager.hpp
+// \author aychar
+// \brief  hpp file for MagnetorquerManager component implementation class
+// ======================================================================
+
+#ifndef Drv_MagnetorquerManager_HPP
+#define Drv_MagnetorquerManager_HPP
+
+#include "FprimeZephyrReference/Components/Drv/MagnetorquerManager/MagnetorquerManagerComponentAc.hpp"
+#include <zephyr/device.h>
+#include <zephyr/drivers/haptics/drv2605.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/kernel.h>
+
+namespace Drv {
+
+class MagnetorquerManager final : public MagnetorquerManagerComponentBase {
+  public:
+    // ----------------------------------------------------------------------
+    // Component construction and destruction
+    // ----------------------------------------------------------------------
+
+    //! Construct MagnetorquerManager object
+    MagnetorquerManager(const char* const compName  //!< The component name
+    );
+
+    //! Destroy MagnetorquerManager object
+    ~MagnetorquerManager();
+
+    //! Configure the DRV2605 device
+    // Accept an array of six pointers to const device objects. The pointers themselves are const
+    // to match callers that provide const device* const* types.
+    void configure(const struct device* const devices[5]);
+
+  private:
+    //! Zephyr device to store initialized DRV2605 devices
+    const struct device* m_devices[5];
+    struct drv2605_rom_data rom = {.library = DRV2605_LIBRARY_TS2200_A, .seq_regs = {47, 0, 0, 0, 0, 0, 0, 0}};
+
+    // Command handlers updated to accept a face index (0..5)
+    void START_PLAYBACK_TEST_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, U8 faceIdx) override;
+    void START_PLAYBACK_TEST2_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, U8 faceIdx) override;
+
+    void SetMagnetorquers_handler(const FwIndexType portNum, const Drv::InputArray& value) override;
+};
+
+}  // namespace Drv
+
+#endif

FprimeZephyrReference/Components/Drv/MagnetorquerManager/docs/sdd.md

@@ -0,0 +1,134 @@
+# Drv::MagnetorquerManager
+
+The Magnetorquer Manager component interfaces with the five DRV2605 devices on a PROVES CubeSat to control the current of the magnetorquers.
+
+## Usage Examples
+
+This component is designed to be used by the detumble service to be able to detumble the cubesat when launched.
+
+### Diagrams
+
+```mermaid
+graph LR
+    A[Detumble Service] -->|SetMagnetorquers| B[MagnetorquerManager]
+    B -->|Configure| C[DRV2605 Device 0<br/>X1 Face]
+    B -->|Configure| D[DRV2605 Device 1<br/>X2 Face]
+    B -->|Configure| E[DRV2605 Device 2<br/>Y1 Face]
+    B -->|Configure| F[DRV2605 Device 3<br/>Y2 Face]
+    B -->|Configure| G[DRV2605 Device 4<br/>Z1 Face]
+```
+
+### Typical Usage
+
+1. The component is instantiated and initialized during system startup
+2. The detumble service calls the `SetMagnetorquers` input port.
+3. On each call, the component:
+   - Takes in an `InputArray` parameter of 5 I32 for the amps for each face.
+   - Translates the passed in values to a sequence value from the DRV2605 library.
+   - Runs the sequence for each device.
+
+## Class Diagram
+
+```mermaid
+classDiagram
+    class MagnetorquerManager {
+        -device* m_devices[5]
+        +configure(devices[5])
+        +SetMagnetorquers_handler(portNum, value)
+        +START_PLAYBACK_TEST_cmdHandler(opCode, cmdSeq, faceIdx)
+        +START_PLAYBACK_TEST2_cmdHandler(opCode, cmdSeq, faceIdx)
+    }
+
+    class MagnetorquerManagerComponentBase {
+        <<F' Component Base>>
+    }
+
+    class DRV2605 {
+        <<Zephyr Driver>>
+        +device_is_ready()
+        +drv2605_haptic_config()
+    }
+
+    MagnetorquerManager --|> MagnetorquerManagerComponentBase
+    MagnetorquerManager --> DRV2605 : uses (5 devices)
+```
+
+## Port Descriptions
+
+| Name             | Description                                                                                 |
+| ---------------- | ------------------------------------------------------------------------------------------- |
+| SetMagnetorquers | Input port that takes in an array (I32[5]) and applies each value to the corresponding face |
+
+## Sequence Diagrams
+
+### SetMagnetorquers Operation
+
+```mermaid
+sequenceDiagram
+    participant DS as Detumble Service
+    participant MM as MagnetorquerManager
+    participant DRV as DRV2605 Devices
+
+    DS->>MM: SetMagnetorquers([x1, x2, y1, y2, z1])
+    Note over MM: Validate input array
+    Note over MM: Translate amps to sequences
+    loop For each face (0-4)
+        MM->>DRV: Check device_is_ready()
+        alt Device Ready
+            MM->>DRV: drv2605_haptic_config(sequence)
+        else Device Not Ready
+            MM->>MM: log_WARNING_HI_DeviceNotReady()
+        end
+    end
+    MM-->>DS: Return
+```
+
+### Test Command Operation
+
+```mermaid
+sequenceDiagram
+    participant GS as Ground Station
+    participant MM as MagnetorquerManager
+    participant DRV as DRV2605 Device
+
+    GS->>MM: START_PLAYBACK_TEST(faceIdx)
+    MM->>MM: Validate faceIdx (0-4)
+    alt Invalid faceIdx
+        MM->>MM: log_WARNING_LO_InvalidFaceIndex()
+    else Valid faceIdx
+        MM->>DRV: device_is_ready()
+        alt Device Ready
+            MM->>DRV: drv2605_haptic_config(effect #47)
+        else Device Not Ready
+            MM->>MM: log_WARNING_HI_DeviceNotReady()
+        end
+    end
+    MM-->>GS: Command Response
+```
+
+## Commands
+
+| Name                 | Description                                                                                      |
+| -------------------- | ------------------------------------------------------------------------------------------------ |
+| START_PLAYBACK_TEST  | Start DRV2605 playback on a device with effect #47 on a specific face (faceIdx: 0-4). Test only. |
+| START_PLAYBACK_TEST2 | Start DRV2605 playback on a device with effect #50 on a specific face (faceIdx: 0-4). Test only. |
+
+## Events
+
+| Name             | Description                                                                                                                                 |
+| ---------------- | ------------------------------------------------------------------------------------------------------------------------------------------- |
+| DeviceNotReady   | Output whenever a magnetorquer is attempted to be used while it is not initialized.                                                         |
+| InvalidFaceIndex | Output whenever one of the manual test comamands are ran with an invalid face index (will be removed if/when the test commands are removed) |
+
+## Requirements
+
+Add requirements in the chart below
+| Name | Description | Validation |
+|---|---|---|
+|---|---|---|
+
+## Change Log
+
+| Date       | Description   |
+| ---------- | ------------- |
+| 11/11/2025 | Initial Draft |