Allows PSim to run without sun vectors (#350)

* Fixed fc model to work without sun vector * Formatting
pathfinder-for-autonomous-navigation · Sep 27, 2021 · 13824df · 13824df
1 parent a481711
commit 13824df
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/src/psim/fc/attitude_estimator.cpp b/src/psim/fc/attitude_estimator.cpp
@@ -76,8 +76,11 @@ void AttitudeEstimator::step() {
   }
   // Attempt to reset the current estimate if it isn't valid.
   else {
-    if (lin::all(lin::isfinite(t)) && lin::all(lin::isfinite(r)) &&
-        lin::all(lin::isfinite(b)) && lin::all(lin::isfinite(s)))
+    if (!lin::all(lin::isfinite(s))) {
+      gnc::attitude_estimator_reset(
+          _attitude_state, t, {0.0f, 0.0f, 0.0f, 1.0f});
+    } else if (lin::all(lin::isfinite(t)) && lin::all(lin::isfinite(r)) &&
+               lin::all(lin::isfinite(b)))
       gnc::attitude_estimator_reset(_attitude_state, t, r, b, s);
   }