attitude orbit model is working in ecef

src/psim/truth/attitude_orbit.cpp

@@ -28,14 +28,14 @@
 
 #include <psim/truth/attitude_orbit.hpp>
 
-#include <gnc/environment.hpp>
 #include <gnc/utilities.hpp>
 
 #include <lin/core.hpp>
 #include <lin/generators/constants.hpp>
 #include <lin/math.hpp>
 #include <lin/references.hpp>
 
+#include <psim/truth/attitude_utilities.hpp>
 #include <psim/truth/orbit_utilities.hpp>
 
 namespace psim {
@@ -49,21 +49,23 @@ void AttitudeOrbitNoFuelEcef::step() {
 
   struct IntegratorData {
     Real const &m;
+    Real const &S;
+    Vector3 const &earth_w;
+    Vector3 const &earth_w_dot;
     Vector3 const &J_body;
-    Vector3 const &wheels_J_body;
+    Real const &wheels_J_body;
     Vector3 const &wheels_t_body;
     Vector3 const &m_body;
     Vector3 const &b_eci;
-    Vector3 const &earth_w;
-    Vector3 const &earth_w_dot;
   };
 
   auto const &dt = truth_dt_s->get();
   auto const &earth_w = truth_earth_w->get();
   auto const &earth_w_dot = truth_earth_w_dot->get();
+  auto const &q_eci_ecef = truth_earth_q_eci_ecef->get();
   auto const &m = truth_satellite_m.get();
   auto const &J_body = truth_satellite_J.get();
-  auto const &wheels_J_body = truth_satellite_J.get();
+  auto const &wheels_J_body = truth_satellite_wheels_J.get();
   auto const &wheels_t_body = truth_satellite_wheels_t.get();
   auto const &b_eci = truth_satellite_environment_b_eci->get();
 
@@ -80,34 +82,39 @@ void AttitudeOrbitNoFuelEcef::step() {
   v_ecef = v_ecef + J_ecef / m;
   J_ecef = lin::zeros<Vector3>();
 
+  // Calculate surface area projected along the direction of travel. It's
+  // this is constant over the course of a timestep.
+  auto const S = attitude::S(q_body_eci, q_eci_ecef, v_ecef);
+
   // Prepare integrator inputs.
   Vector<16> x;
   lin::ref<Vector3>(x, 0, 0) = r_ecef;
   lin::ref<Vector3>(x, 3, 0) = v_ecef;
   lin::ref<Vector4>(x, 6, 0) = q_body_eci;
   lin::ref<Vector3>(x, 10, 0) = w_body;
   lin::ref<Vector3>(x, 13, 0) = wheels_w_body;
-  IntegratorData data{m, J_body, wheels_J_body, wheels_t_body, m_body, b_eci,
-      earth_w, earth_w_dot};
+  IntegratorData data{m, S, earth_w, earth_w_dot, J_body, wheels_J_body,
+      wheels_t_body, m_body, b_eci};
 
   // Simulate dynamics.
   x = ode(Real(0.0), dt, x, &data,
       [](Real t, Vector<16> const &x, void *ptr) -> Vector<16> {
         auto const *data = static_cast<IntegratorData *>(ptr);
 
         auto const &m = data->m;
+        auto const &S = data->S;
         auto const earth_w = (data->earth_w + t * data->earth_w_dot).eval();
         auto const &earth_w_dot = data->earth_w_dot;
+        auto const &J_body = data->J_body;
+        auto const &wheels_J_body = data->wheels_J_body;
+        auto const &wheels_t_body = data->wheels_t_body;
+        auto const &m_body = data->m_body;
 
         auto const r_ecef = lin::ref<Vector3>(x, 0, 0);
         auto const v_ecef = lin::ref<Vector3>(x, 3, 0);
         auto const q_body_eci = lin::ref<Vector4>(x, 6, 0);
         auto const w_body = lin::ref<Vector3>(x, 10, 0);
         auto const wheels_w_body = lin::ref<Vector3>(x, 13, 0);
-
-        auto const S = 0.0; [&q_body_eci]() {
-
-        }();
         auto const b_body = [&q_body_eci](Vector3 const &b_eci) {
           Vector3 b_body;
           gnc::utl::rotate_frame(q_body_eci.eval(), b_eci, b_body);
@@ -118,33 +125,39 @@ void AttitudeOrbitNoFuelEcef::step() {
 
         // Orbital dynamics
         {
-          Vector3 a_ecef;
-          orbit::acceleration(
-              earth_w, earth_w_dot, r_ecef.eval(), v_ecef.eval(), S, m, a_ecef);
+          Vector3 const a_ecef = orbit::acceleration(
+              earth_w, earth_w_dot, r_ecef.eval(), v_ecef.eval(), S, m);
 
           lin::ref<Vector3>(dx, 0, 0) = v_ecef;
           lin::ref<Vector3>(dx, 3, 0) = a_ecef;
         }
 
-        // dq = utl_quat_cross_mult(0.5*quat_rate,quat_body_eci);
-        Vector4 dq;
-        Vector4 quat_rate = {0.5 * w(0), 0.5 * w(1), 0.5 * w(2), 0.0};
-        gnc::utl::quat_cross_mult(quat_rate, q.eval(), dq);
+        // Attitude dynamics - quaternion
+        {
+          Vector4 dq_body_eci;
+          Vector4 const dq = {
+              0.5 * w_body(0), 0.5 * w_body(1), 0.5 * w_body(2), 0.0};
+          gnc::utl::quat_cross_mult(dq, q_body_eci.eval(), dq_body_eci);
 
-        // dw = I^{-1} * m x b - tau_w - w x (I * w + I_w * w_w)
-        Vector3 dw =
-            lin::cross(data->m, data->b) - data->tau_w -
-            lin::cross(w, lin::multiply(data->I, w) + (data->I_w) * w_w);
-        dw = lin::divide(dw, data->I); // Multiplication by I inverse
+          lin::ref<Vector4>(dx, 6, 0) = dq_body_eci;
+        }
+
+        // Attitude dynamics - angular rate
+        {
+          Vector3 const H_body =
+              lin::multiply(J_body, w_body) + wheels_J_body * wheels_w_body;
+          Vector3 const t_body = lin::cross(m_body, b_body) - wheels_t_body -
+                                 lin::cross(w_body, H_body);
+          Vector3 const dw_body = lin::divide(t_body, J_body);
 
-        // dw_w = tau_w/I_w
-        Vector3 dw_w = lin::divide(data->tau_w, data->I_w);
+          lin::ref<Vector3>(dx, 10, 0) = dw_body;
+        }
 
-        // Attitude dynamics
+        // Attitude dynamics - reaction wheel rates
         {
-          lin::ref<Vector4>(dx, 6, 0) = dq;
-          lin::ref<Vector3>(dx, 10, 0) = dw;
-          lin::ref<Vector3>(dx, 13, 0) = dw_w;
+          Vector3 const dwheels_w_body = wheels_t_body / wheels_J_body;
+
+          lin::ref<Vector3>(dx, 13, 0) = dwheels_w_body;
         }
 
         return dx;
@@ -158,6 +171,35 @@ void AttitudeOrbitNoFuelEcef::step() {
   wheels_w_body = lin::ref<Vector3>(x, 13, 0);
 }
 
+Real AttitudeOrbitNoFuelEcef::truth_satellite_orbit_T() const {
+  static constexpr Real half = 0.5;
+
+  auto const &earth_w = truth_earth_w->get();
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &v_ecef = truth_satellite_orbit_v.get();
+  auto const &m = truth_satellite_m.get();
+
+  return half * m * lin::fro(v_ecef + lin::cross(earth_w, r_ecef));
+}
+
+Real AttitudeOrbitNoFuelEcef::truth_satellite_orbit_U() const {
+  auto const &r_ecef = truth_satellite_orbit_r.get();
+  auto const &m = truth_satellite_m.get();
+
+  Real U;
+  Vector3 _;
+  orbit::gravity(r_ecef, _, U);
+
+  return m * U;
+}
+
+Real AttitudeOrbitNoFuelEcef::truth_satellite_orbit_E() const {
+  auto const &T = this->Super::truth_satellite_orbit_T.get();
+  auto const &U = this->Super::truth_satellite_orbit_U.get();
+
+  return T - U;
+}
+
 Vector4 AttitudeOrbitNoFuelEcef::truth_satellite_attitude_q_eci_body() const {
   auto const &q_body_eci = this->truth_satellite_attitude_q_body_eci.get();
 

src/psim/truth/attitude_utilities.cpp

@@ -39,13 +39,13 @@ namespace attitude {
 
 Real S(Vector4 const &q_body_eci, Vector4 const &q_eci_ecef,
     Vector3 const &v_ecef) {
-  static constexpr Vector3 PSIM_AREA_VEC = {0.03, 0.03, 0.01};
+  static constexpr Vector3 A = {0.03, 0.03, 0.01};
 
   Vector3 v_body;
   gnc::utl::rotate_frame(q_eci_ecef, v_ecef, v_body);
   gnc::utl::rotate_frame(q_body_eci, v_body);
 
-  return lin::dot(lin::abs(v_body / lin::norm(v_body)), PSIM_AREA_VEC);
+  return lin::dot(lin::abs(v_body / lin::norm(v_body)), A);
 }
 } // namespace attitude
 } // namespace psim

src/psim/truth/attitude_utilities.hpp

@@ -34,13 +34,17 @@
 namespace psim {
 namespace attitude {
 
-/** @brief
+/** @brief Calculates satellite surface area projected along the direction of
+ *         travel.
  *
- *  @param[in]
+ *  @param[in] q_body_eci Rotation from ECI to body.
+ *  @param[in] q_eci_ecef Rotation from ECEF to ECI.
+ *  @param[in] v_ecef     Velocity in ECEC (m/s).
  *
- *  @return
+ *  @return Surface area projected along the direction of travel (m^2).
  */
-Real S(Vector4 const &q_body_eci, Vector4 const &q_eci_ecef, Vector3 const &v_ecef);
+Real S(Vector4 const &q_body_eci, Vector4 const &q_eci_ecef,
+    Vector3 const &v_ecef);
 
 } // namespace attitude
 } // namespace psim

src/psim/truth/satellite_truth.cpp

@@ -43,9 +43,9 @@ SatelliteTruthGnc::SatelliteTruthGnc(RandomsGenerator &randoms,
     Configuration const &config, std::string const &satellite)
   : ModelList(randoms) {
   // Dynamics
-  add<AttitudeOrbitNoFuelEciGnc>(randoms, config, satellite);
-  add<TransformPositionEci>(randoms, config, "truth." + satellite + ".orbit.r");
-  add<TransformVelocityEci>(randoms, config, satellite, "truth." + satellite + ".orbit.v");
+  add<AttitudeOrbitNoFuelEcef>(randoms, config, satellite);
+  add<TransformPositionEcef>(randoms, config, "truth." + satellite + ".orbit.r");
+  add<TransformVelocityEcef>(randoms, config, satellite, "truth." + satellite + ".orbit.v");
   // Extra telemetry
   add<NormVector3>(randoms, config, "truth." + satellite + ".attitude.L");
   add<NormVector4>(randoms, config, "truth." + satellite + ".attitude.q.body_eci");