add setDipoleMoment function

Author: hrfarmer

FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.cpp

@@ -7,6 +7,9 @@
 
 #include <Fw/Types/Assert.hpp>
 
+#include <algorithm>
+#include <cmath>
+
 namespace Components {
 
 // ----------------------------------------------------------------------
@@ -48,4 +51,24 @@ bool DetumbleManager::executeControlStep() {
     // Then apply the dipole moment here, gonna have to figure that out.
     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::min(std::fabs(unlimited_x), this->COIL_MAX_CURRENT_X_Y) * (unlimited_x >= 0 ? 1.0f : -1.0f);
+    F64 limited_y = std::min(std::fabs(unlimited_y), this->COIL_MAX_CURRENT_X_Y) * (unlimited_y >= 0 ? 1.0f : -1.0f);
+    F64 limited_z = std::min(std::fabs(unlimited_z), this->COIL_MAX_CURRENT_Z) * (unlimited_z >= 0 ? 1.0f : -1.0f);
+
+    F64 x1 = limited_x;
+    F64 x2 = -limited_x;
+    F64 y1 = limited_y;
+    F64 y2 = -limited_y;
+    F64 z1 = limited_z;
+
+    // Apply values to magnetorquers here
+}
+
 }  // namespace Components

FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.hpp

@@ -8,6 +8,8 @@
 
 #include "FprimeZephyrReference/Components/DetumbleManager/DetumbleManagerComponentAc.hpp"
 
+#include <cmath>
+
 namespace Components {
 
 class DetumbleManager final : public DetumbleManagerComponentBase {
@@ -32,14 +34,31 @@ class DetumbleManager final : public DetumbleManagerComponentBase {
                      U32 context           //!< The call order
                      ) override;
 
-    // Variables
+    // Constants
     Drv::MagneticField EMPTY_MG_FIELD = Drv::MagneticField(0.0, 0.0, 0.0, -1);
-    Drv::MagneticField prevMgField = 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 * powf(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);
 
     // Functions
     bool executeControlStep();
+    void setDipoleMoment(Drv::DipoleMoment dpMoment);
 };
 
 }  // namespace Components