From 13824dfb94d07a672ce1e97e89e205b815bf1584 Mon Sep 17 00:00:00 2001 From: Govind Chari Date: Mon, 27 Sep 2021 01:26:58 -0400 Subject: [PATCH] Allows PSim to run without sun vectors (#350) * Fixed fc model to work without sun vector * Formatting --- src/psim/fc/attitude_estimator.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/psim/fc/attitude_estimator.cpp b/src/psim/fc/attitude_estimator.cpp index 4861681d..fdd7ce5a 100644 --- 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); }