Detumble

FprimeZephyrReference/Components/CMakeLists.txt

@@ -6,8 +6,9 @@ add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Burnwire/")
 add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ComDelay/")
 add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Drv/")
 add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/FatalHandler")
+add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/DetumbleManager/")
 add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/FsSpace/")
-add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/ImuManager/")
 add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/NullPrmDb/")
 add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/PowerMonitor/")
 add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/Watchdog")
+add_fprime_subdirectory("${CMAKE_CURRENT_LIST_DIR}/LoadSwitch/")

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

FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.cpp

@@ -0,0 +1,117 @@
+// ======================================================================
+// \title  DetumbleManager.cpp
+// \brief  cpp file for DetumbleManager component implementation class
+// ======================================================================
+
+#include "FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.hpp"
+
+#include <Fw/Types/Assert.hpp>
+#include <algorithm>
+#include <cmath>
+
+namespace Components {
+
+// ----------------------------------------------------------------------
+// Component construction and destruction
+// ----------------------------------------------------------------------
+
+DetumbleManager ::DetumbleManager(const char* const compName) : DetumbleManagerComponentBase(compName) {}
+
+DetumbleManager ::~DetumbleManager() {}
+
+// ----------------------------------------------------------------------
+// Handler implementations for typed input ports
+// ----------------------------------------------------------------------
+
+void DetumbleManager ::run_handler(FwIndexType portNum, U32 context) {
+    U32 currTime = this->getTime().getSeconds();
+
+    // I'm not checking the value of isValid for paramGet calls since all of the parameters have a
+    // default value, but maybe there should still be specific logging for if the default parameter
+    // was used and retrieval failed.
+    Fw::ParamValid isValid;
+
+    if (this->detumbleRunning) {
+        // Check if we've hit the max time detumble can run
+        U32 maxTime = this->paramGet_MAX_TIME(isValid);
+        if (currTime - this->startTime <= maxTime) {
+            bool res = this->executeControlStep();
+            if (!res) {
+                // Log some error
+            }
+
+            this->iterations++;
+
+            // Every 10 iterations (every second), check if the rotation per second is less than 12 degrees
+            if (this->iterations % 10 == 0) {
+                Drv::AngularVelocity angVel = this->angularVelocityGet_out(0);
+                F64 rotationRate = this->getAngularVelocityMagnitude(angVel);
+
+                F64 threshold = this->paramGet_ROTATIONAL_THRESHOLD(isValid);
+                if (rotationRate < threshold) {
+                    // Rotation is below threshold - can stop detumbling
+                    this->detumbleRunning = false;
+                    this->lastCompleted = currTime;
+                }
+            }
+        } else {
+            // Max time reached, disable detumble for now
+            this->detumbleRunning = false;
+            this->lastCompleted = currTime;
+        }
+    } else {
+        // Check if the cooldown has ended, and start if so.
+        if (currTime - this->lastCompleted >= this->paramGet_COOLDOWN(isValid)) {
+            this->detumbleRunning = true;
+            this->startTime = currTime;
+        }
+    }
+}
+
+bool DetumbleManager::executeControlStep() {
+    Drv::MagneticField mgField = this->magneticFieldGet_out(0);
+    if (this->prevMgField == this->EMPTY_MG_FIELD) {
+        this->prevMgField = mgField;
+    }
+
+    Drv::DipoleMoment dpMoment = this->dipoleMomentGet_out(0, mgField, this->prevMgField);
+    if (dpMoment == this->EMPTY_DP_MOMENT) {
+        // Log some kinda error
+        return false;
+    }
+
+    this->prevMgField = mgField;
+    this->setDipoleMoment(dpMoment);
+
+    return true;
+}
+void DetumbleManager::setDipoleMoment(Drv::DipoleMoment dpMoment) {
+    // Convert dipole moment to (unlimited) current
+    F64 unlimited_x = dpMoment.get_x() / (this->COIL_NUM_TURNS_X_Y * this->COIL_AREA_X_Y);
+    F64 unlimited_y = dpMoment.get_y() / (this->COIL_NUM_TURNS_X_Y * this->COIL_AREA_X_Y);
+    F64 unlimited_z = dpMoment.get_z() / (this->COIL_NUM_TURNS_Z * this->COIL_AREA_Z);
+
+    // Limit current for each axis to max coil current
+    F64 limited_x = std::fmin(std::fabs(unlimited_x), this->COIL_MAX_CURRENT_X_Y) * (unlimited_x >= 0 ? 1.0 : -1.0);
+    F64 limited_y = std::fmin(std::fabs(unlimited_y), this->COIL_MAX_CURRENT_X_Y) * (unlimited_y >= 0 ? 1.0 : -1.0);
+    F64 limited_z = std::fmin(std::fabs(unlimited_z), this->COIL_MAX_CURRENT_Z) * (unlimited_z >= 0 ? 1.0 : -1.0);
+
+    F64 x1 = limited_x;
+    F64 x2 = -limited_x;
+    F64 y1 = limited_y;
+    F64 y2 = -limited_y;
+    F64 z1 = limited_z;
+
+    this->magnetorquersSet_out(0, {x1, x2, y1, y2, z1});
+}
+
+F64 DetumbleManager::getAngularVelocityMagnitude(const Drv::AngularVelocity& angVel) {
+    // Calculate magnitude in rad/s
+    F64 magRadPerSec =
+        std::sqrt(angVel.get_x() * angVel.get_x() + angVel.get_y() * angVel.get_y() + angVel.get_z() * angVel.get_z());
+
+    // Convert rad/s to deg/s
+    return magRadPerSec * 180.0 / this->PI;
+}
+
+}  // namespace Components

FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.fpp

@@ -0,0 +1,62 @@
+module Components {
+    @ Detumble Manager Component for F Prime FSW framework.
+    passive component DetumbleManager {
+
+        @ Parameter for storing the rotational threshold for detumble to be enabled (default 12 deg/s)
+        param ROTATIONAL_THRESHOLD: F32 default 12.0 \
+            id 1
+
+        @ Parameter for storing the max amount of time an iteration can run
+        param MAX_TIME: U32 default 10800 \
+            id 2
+
+        @ Parameter for storing the cooldown after a detumble run finishes (in seconds)
+        param COOLDOWN: U32 default 600 \
+            id 3
+
+        @ Run loop
+        sync input port run: Svc.Sched
+
+        @ Port for sending accelerationGet calls to the LSM6DSO Driver
+        output port accelerationGet: Drv.AccelerationGet
+
+        @ Port for sending angularVelocityGet calls to the LSM6DSO Driver
+        output port angularVelocityGet: Drv.AngularVelocityGet
+
+        @ Port for sending magneticFieldGet calls to the LIS2MDL Manager
+        output port magneticFieldGet: Drv.MagneticFieldGet
+
+        @ Port for sending temperatureGet calls to the LSM6DSO Driver
+        output port temperatureGet: Drv.TemperatureGet
+
+        @ Port for sending dipoleMomentGet calls to the BDotDetumble Component
+        output port dipoleMomentGet: Drv.DipoleMomentGet
+
+        @ Port for sending SetMagnetorquers calls to the MagnetorquerManager Component
+        output port magnetorquersSet: Drv.SetMagnetorquers
+
+        ###############################################################################
+        # Standard AC Ports: Required for Channels, Events, Commands, and Parameters  #
+        ###############################################################################
+        @ Port for requesting the current time
+        time get port timeCaller
+
+        @ Port for getting parameters
+        param get port prmGetOut
+
+        @ Port for setting parameters
+        param set port prmSetOut
+
+        @ 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
+
+
+
+    }
+}

FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.hpp

@@ -0,0 +1,71 @@
+// ======================================================================
+// \title  DetumbleManager.hpp
+// \brief  hpp file for DetumbleManager component implementation class
+// ======================================================================
+
+#ifndef Components_DetumbleManager_HPP
+#define Components_DetumbleManager_HPP
+
+#include <cmath>
+
+#include "FprimeZephyrReference/Components/DetumbleManager/DetumbleManagerComponentAc.hpp"
+
+namespace Components {
+
+class DetumbleManager final : public DetumbleManagerComponentBase {
+  public:
+    // ----------------------------------------------------------------------
+    // Component construction and destruction
+    // ----------------------------------------------------------------------
+
+    //! Construct DetumbleManager object
+    DetumbleManager(const char* const compName);
+
+    //! Destroy DetumbleManager object
+    ~DetumbleManager();
+
+  private:
+    // ----------------------------------------------------------------------
+    // Handler implementations for typed input ports
+    // ----------------------------------------------------------------------
+
+    //! Handler implementation
+    void run_handler(FwIndexType portNum,  //!< The port number
+                     U32 context           //!< The call order
+                     ) override;
+
+    // Constants
+    Drv::MagneticField EMPTY_MG_FIELD = Drv::MagneticField(0.0, 0.0, 0.0, -1);
+    Drv::DipoleMoment EMPTY_DP_MOMENT = Drv::DipoleMoment(0.0, 0.0, 0.0);
+    const double PI = 3.14159265358979323846;
+
+    // Proves V3 Magnetorquer Information
+    F64 COIL_VOLTAGE = 3.3;
+    F64 COIL_NUM_TURNS_X_Y = 48;
+    F64 COIL_LENGTH_X_Y = 0.053;
+    F64 COIL_WIDTH_X_Y = 0.045;
+    F64 COIL_AREA_X_Y = this->COIL_LENGTH_X_Y * this->COIL_WIDTH_X_Y;
+    F64 COIL_RESISTANCE_X_Y = 57.2;
+    F64 COIL_MAX_CURRENT_X_Y = this->COIL_VOLTAGE / this->COIL_RESISTANCE_X_Y;
+    I64 COIL_NUM_TURNS_Z = 153;
+    F64 COIL_DIAMETER_Z = 0.05755;
+    F64 COIL_AREA_Z = this->PI * pow(this->COIL_DIAMETER_Z / 2, 2.0);
+    F64 COIL_RESISTANCE_Z = 248.8;
+    F64 COIL_MAX_CURRENT_Z = this->COIL_VOLTAGE / this->COIL_RESISTANCE_Z;
+
+    // Variables
+    Drv::MagneticField prevMgField = Drv::MagneticField(0.0, 0.0, 0.0, -1);
+    int iterations = 0;
+    U32 startTime = 0;
+    U32 lastCompleted = 0;
+    bool detumbleRunning = true;
+
+    // Functions
+    bool executeControlStep();
+    void setDipoleMoment(Drv::DipoleMoment dpMoment);
+    F64 getAngularVelocityMagnitude(const Drv::AngularVelocity& angVel);
+};
+
+}  // namespace Components
+
+#endif

FprimeZephyrReference/Components/DetumbleManager/docs/sdd.md

@@ -0,0 +1,73 @@
+# Components::DetumbleManager
+
+The IMU Manager (Inertial Measurement Unit) component provides sensor data related to motion and orientation of the craft. It interfaces with two drivers: the LIS2MDL Manager and the LSM6DSO Driver which provide acceleration, angular velocity, magnetic field, and temperature measurements.
+
+## Usage Examples
+
+The IMU Manager component is designed to be scheduled periodically to trigger collection of sensor data and telemetering. It operates as a passive component that responds to scheduler calls.
+
+### Typical Usage
+
+1. The component is instantiated and initialized during system startup
+2. The scheduler calls the `run` port at regular intervals
+3. On each run call, the component:
+   - Fetches sensor data from both the LIS2MDL Manager and LSM6DSO Driver
+   - Outputs telemetry for acceleration, angular velocity, magnetic field, and temperature
+
+## Class Diagram
+
+```mermaid
+classDiagram
+    namespace Components {
+        class DetumbleManagerComponentBase {
+            <<Auto-generated>>
+        }
+        class DetumbleManager {
+            + DetumbleManager(const char* compName)
+            + ~DetumbleManager()
+            - run_handler(FwIndexType portNum, U32 context): void
+        }
+    }
+    DetumbleManagerComponentBase <|-- DetumbleManager : inherits
+```
+
+## Port Descriptions
+
+| Name               | Type       | Description                                                |
+| ------------------ | ---------- | ---------------------------------------------------------- |
+| run                | sync input | Scheduler port that triggers sensor data collection        |
+| accelerationGet    | output     | Port for calling accelerationGet on the LSM6DSO Manager    |
+| angularVelocityGet | output     | Port for calling angularVelocityGet on the LSM6DSO Manager |
+| magneticFieldGet   | output     | Port for calling magneticFieldGet on the LIS2MDL Manager   |
+| temperatureGet     | output     | Port for calling temperatureGet on the LSM6DSO Manager     |
+| timeCaller         | time get   | Port for requesting current system time                    |
+
+## Sequence Diagrams
+
+```mermaid
+sequenceDiagram
+    participant Scheduler
+    participant IMU Manager
+    participant LIS2MDL Manager
+    participant LSM6DSO Manager
+
+    Scheduler-->>IMU Manager: run
+    IMU Manager->>LSM6DSO Manager: AccelerationGet
+    IMU Manager->>LSM6DSO Manager: AngularVelocityGet
+    IMU Manager->>LSM6DSO Manager: TemperatureGet
+    IMU Manager->>LIS2MDL Manager: MagneticFieldGet
+```
+
+## Requirements
+
+| Name                   | Description                                                                                          | Validation                                             |
+| ---------------------- | ---------------------------------------------------------------------------------------------------- | ------------------------------------------------------ |
+| Sensor Data Collection | The component shall trigger data collection from both LSM6DSO and LIS2MDL sensors when run is called | Verify all sensor manager output ports are called      |
+| Periodic Operation     | The component shall operate as a scheduled component responding to scheduler calls                   | Verify component responds correctly to scheduler input |
+
+## Change Log
+
+| Date      | Description                                                           |
+| --------- | --------------------------------------------------------------------- |
+| 2025-9-9  | Initial IMU Manager component                                         |
+| 2025-9-18 | Extracted Zephyr calls to discrete LIS2MDL Manager and LSM6DSO Driver |