Implemented BDotDetumble

Author: hrfarmer

FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.cpp

@@ -30,4 +30,18 @@ void DetumbleManager ::run_handler(FwIndexType portNum, U32 context) {
     // Read from Lis2mdlManager
     this->magneticFieldGet_out(0);
 }
+
+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_handler(0, mgField, this->prevMgField);
+    // Then check if its null/empty whatever here, need to figure that out.
+    this->prevMgField = mgField;
+
+    // Then apply the dipole moment here, gonna have to figure that out.
+    return true;
+}
 }  // namespace Components

FprimeZephyrReference/Components/DetumbleManager/DetumbleManager.hpp

@@ -31,6 +31,13 @@ class DetumbleManager final : public DetumbleManagerComponentBase {
     void run_handler(FwIndexType portNum,  //!< The port number
                      U32 context           //!< The call order
                      ) override;
+
+    // Variables
+    Drv::MagneticField EMPTY_MG_FIELD = Drv::MagneticField{0.0, 0.0, 0.0};
+    Drv::MagneticField prevMgField = Drv::MagneticField{0.0, 0.0, 0.0};
+
+    // Functions
+    bool executeControlStep();
 };
 
 }  // namespace Components

FprimeZephyrReference/Components/Drv/BDotDetumble/BDotDetumble.cpp

@@ -18,21 +18,21 @@ BDotDetumble ::BDotDetumble(const char* const compName) : BDotDetumbleComponentB
 
 BDotDetumble ::~BDotDetumble() {}
 
-Drv::DipoleMoment dipoleMomentGet_handler(const FwIndexType portNum,
-                                          const Drv::MagneticField currMagField,
-                                          const Drv::MagneticField prevMagField) {
-    F32 magnitude = getMagnitude(currMagField);
+Drv::DipoleMoment BDotDetumble::dipoleMomentGet_handler(const FwIndexType portNum,
+                                                        const Drv::MagneticField& currMagField,
+                                                        const Drv::MagneticField& prevMagField) {
+    F64 magnitude = this->getMagnitude(currMagField);
     if (magnitude < 1e-6) {
-        return;  // Figure out how I should handle the return here.
+        return Drv::DipoleMoment();
     }
 
-    if (currMagField.timestamp <= prevMagField.timestamp) {
-        return;  // Also figure out the return here.
+    if (currMagField.get_timestamp() <= prevMagField.get_timestamp()) {
+        return Drv::DipoleMoment();
     }
 
-    F64* dB_dtArr = dB_dt(currMagField, prevMagField);
+    F64* dB_dtArr = this->dB_dt(currMagField, prevMagField);
     if (dB_dtArr == nullptr) {
-        return;  // Yeah just figure out how to properly raise error/return nothing.
+        return Drv::DipoleMoment();
     }
 
     F64 moment_x = this->gain * dB_dtArr[0] / magnitude;
@@ -44,19 +44,19 @@ Drv::DipoleMoment dipoleMomentGet_handler(const FwIndexType portNum,
     return Drv::DipoleMoment(moment_x, moment_y, moment_z);
 }
 
-F32 getMagnitude(const Drv::MagneticField magField) {
-    return sqrt(f64_square(magField.x) + f64_square(magField.y) + f64_square(magField.z));
+F64 BDotDetumble::getMagnitude(const Drv::MagneticField magField) {
+    return sqrt(f64_square(magField.get_x()) + f64_square(magField.get_y()) + f64_square(magField.get_z()));
 }
 
-F64* dB_dt(const Drv::MagneticField currMagField, const Drv::MagneticField prevMagField) {
-    I64 dt = currMagField.timestamp - prevMagField.timestamp;
+F64* BDotDetumble::dB_dt(const Drv::MagneticField currMagField, const Drv::MagneticField prevMagField) {
+    I64 dt = currMagField.get_timestamp() - prevMagField.get_timestamp();
     if (dt < 1e-6) {
         return nullptr;
     }
 
-    F64 dBx_dt = (currMagField.x - prevMagField.x) / dt;
-    F64 dBy_dt = (currMagField.y - prevMagField.y) / dt;
-    F64 dBz_dt = (currMagField.z - prevMagField.z) / dt;
+    F64 dBx_dt = (currMagField.get_x() - prevMagField.get_x()) / dt;
+    F64 dBy_dt = (currMagField.get_y() - prevMagField.get_y()) / dt;
+    F64 dBz_dt = (currMagField.get_z() - prevMagField.get_z()) / dt;
 
     F64* arr = new F64[3];
     arr[0] = dBx_dt;

FprimeZephyrReference/Components/Drv/BDotDetumble/BDotDetumble.fpp

@@ -1,5 +1,8 @@
 module Drv {
-    port DipoleMomentGet -> DipoleMoment
+    port DipoleMomentGet(
+        currMagField: MagneticField
+        prevMagField: MagneticField
+    ) -> DipoleMoment
 }
 
 module Drv {

FprimeZephyrReference/Components/Drv/BDotDetumble/BDotDetumble.hpp

@@ -26,17 +26,18 @@ class BDotDetumble final : public BDotDetumbleComponentBase {
     ~BDotDetumble();
 
     // Get the current dipole moment
-    Drv::DipoleMoment dipoleMomentGet_handler(const FwIndexType portNum) override;
+    Drv::DipoleMoment dipoleMomentGet_handler(const FwIndexType portNum,
+                                              const Drv::MagneticField& currMagField,
+                                              const Drv::MagneticField& prevMagField) override;
 
   private:
     F64 gain = 1.0;
 
     // Get magnitude
-    F32 getMagnitude(const Drv::MagneticField magField);
+    F64 getMagnitude(Drv::MagneticField magField);
 
     // Get the time derivative of the magnetic field
-    F64* dB_dt(const Drv::MagneticField currMagField, const Drv::MagneticField prevMagField);
-
+    F64* dB_dt(Drv::MagneticField currMagField, Drv::MagneticField prevMagField);
+};
 }  // namespace Drv
-
 #endif

FprimeZephyrReference/Components/Drv/Lis2mdlManager/Lis2mdlManager.cpp

@@ -27,7 +27,7 @@ Lis2mdlManager ::~Lis2mdlManager() {}
 Drv::MagneticField Lis2mdlManager ::magneticFieldGet_handler(FwIndexType portNum) {
     if (!device_is_ready(dev)) {
         this->log_WARNING_HI_DeviceNotReady();
-        return Drv::MagneticField(0.0, 0.0, 0.0);
+        return Drv::MagneticField(0.0, 0.0, 0.0, k_uptime_get());
     }
     this->log_WARNING_HI_DeviceNotReady_ThrottleClear();
 

FprimeZephyrReference/Components/Drv/Types/Types.fpp

@@ -22,6 +22,7 @@ module Drv {
         timestamp: I64 @< Time the magnetic field struct was created. Used for dB_dt calculation.
     }
 
+    @ Calculated dipole moment from the B-Dot algorithm.
     struct DipoleMoment {
         x: F64
         y: F64