Jayden's branch

CMakeLists.txt

@@ -1,16 +1,22 @@
-cmake_minimum_required(VERSION 3.10)
-project(MyProject LANGUAGES CXX)
+cmake_minimum_required(VERSION 3.16)
 
+# Small C++ project for the ADCS simulation.
+project(adcs_faithful LANGUAGES CXX)
+
+# Keep the compiler settings simple and explicit.
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-find_package(GeographicLib REQUIRED) # required for magentic field model
-
-# Source files
-file(GLOB SOURCES "src/*.cpp")
-
-add_executable(${PROJECT_NAME} ${SOURCES})
-
-target_include_directories(${PROJECT_NAME} PRIVATE include)
+# Build one executable from the core simulation modules.
+add_executable(adcs
+  src/main.cpp
+  src/satellite.cpp
+  src/control.cpp
+  src/sensor.cpp
+  src/navigation.cpp
+  src/disturbance.cpp
+  src/wmm.cpp
+)
 
-target_link_libraries(${PROJECT_NAME} PRIVATE GeographicLib::GeographicLib)  # required for magentic field model
+# All headers live in the local include folder.
+target_include_directories(adcs PRIVATE include)

README_WMM_UPDATE.txt

@@ -0,0 +1,43 @@
+This package updates the earlier magnetorquer-only ADCS simulation to use WMM2025.
+
+What changed
+- Replaced the old placeholder magnetic model with a real WMM2025 spherical-harmonic model.
+- Added ECI <-> ECEF rotation so the field is evaluated in an Earth-fixed frame.
+- Added ECEF -> geodetic latitude / longitude / altitude conversion on WGS84.
+- Kept the rest of the closed-loop flow the same: sensors -> nav filter -> controller -> magnetorquer torque.
+- Kept comments plain and direct so the code is easier to follow.
+
+Field pipeline
+1. Orbit state is propagated in an inertial frame (ECI-like).
+2. Position is rotated into ECEF using Earth rotation.
+3. ECEF position is converted to geodetic lat/lon/alt.
+4. WMM2025 returns local North/East/Down magnetic field in nanoTesla.
+5. That local field is turned into an ECEF vector, then rotated back to ECI, then into the body frame.
+6. The body-frame field is stored in Tesla for torque calculations.
+
+Build
+- Open a terminal in the folder that contains CMakeLists.txt.
+- Run: cmake -S . -B build
+- Run: cmake --build build
+- Run: .\build\adcs.exe   (Windows)
+- Then run: python .\plot_all.py
+
+Notes
+- This is a much better magnetic environment model than the old placeholder, but it is still not a full flight-quality simulator.
+- The disturbance and sensor models are still simple.
+- The navigation block is still just a smoothing filter, not a real estimator.
+
+If you get an error, run 
+Remove-Item -Recurse -Force build -ErrorAction SilentlyContinue
+cmake -S . -B build -G "MinGW Makefiles"
+cmake --build build
+if (Test-Path .\build\adcs.exe) {
+    .\build\adcs.exe
+} elseif (Test-Path .\build\Debug\adcs.exe) {
+    .\build\Debug\adcs.exe
+} elseif (Test-Path .\build\Release\adcs.exe) {
+    .\build\Release\adcs.exe
+} else {
+    Write-Host "adcs.exe not found after build"
+}
+python .\plot_all.py

include/frames.hpp

@@ -0,0 +1,119 @@
+#pragma once
+#include "math.hpp"
+#include <cmath>
+
+// 3-2-1 Euler rotation matrix.
+// This maps a vector from body coordinates to inertial coordinates.
+inline Mat3 TIB(double phi, double theta, double psi) {
+  const double ct = std::cos(theta);
+  const double st = std::sin(theta);
+  const double sp = std::sin(phi);
+  const double cp = std::cos(phi);
+  const double ss = std::sin(psi);
+  const double cs = std::cos(psi);
+
+  Mat3 out{};
+  out[0][0] = ct * cs;
+  out[0][1] = sp * st * cs - cp * ss;
+  out[0][2] = cp * st * cs + sp * ss;
+
+  out[1][0] = ct * ss;
+  out[1][1] = sp * st * ss + cp * cs;
+  out[1][2] = cp * st * ss - sp * cs;
+
+  out[2][0] = -st;
+  out[2][1] = sp * ct;
+  out[2][2] = cp * ct;
+  return out;
+}
+
+// Small helper used by some of the old code.
+inline Mat3 Rscrew(const Vec3& nhat) {
+  const double x = nhat.x;
+  const double y = nhat.y;
+  const double z = nhat.z;
+  const double psi = std::atan2(y, x);
+  const double theta = std::atan2(z, std::sqrt(x * x + y * y));
+  const double phi = 0.0;
+  return transpose(TIB(phi, theta, psi));
+}
+
+// Rotate a vector about the Earth Z axis.
+inline Mat3 Rz(double theta) {
+  const double c = std::cos(theta);
+  const double s = std::sin(theta);
+  Mat3 R{};
+  R[0][0] = c;  R[0][1] = -s; R[0][2] = 0.0;
+  R[1][0] = s;  R[1][1] =  c; R[1][2] = 0.0;
+  R[2][0] = 0.0;R[2][1] = 0.0;R[2][2] = 1.0;
+  return R;
+}
+
+// Convert an inertial vector into Earth-fixed coordinates.
+inline Vec3 eci_to_ecef(const Vec3& r_eci, double earth_angle_rad) {
+  return mul(Rz(earth_angle_rad), r_eci);
+}
+
+// Convert an Earth-fixed vector back into inertial coordinates.
+inline Vec3 ecef_to_eci(const Vec3& v_ecef, double earth_angle_rad) {
+  return mul(Rz(-earth_angle_rad), v_ecef);
+}
+
+// Simple geodetic container.
+struct GeodeticLLA {
+  double lat_rad{0.0};
+  double lon_rad{0.0};
+  double alt_m{0.0};
+};
+
+// Convert ECEF position to geodetic latitude, longitude, and height above WGS84.
+inline GeodeticLLA ecef_to_geodetic_wgs84(const Vec3& r_ecef) {
+  const double a = 6378137.0;
+  const double f = 1.0 / 298.257223563;
+  const double e2 = f * (2.0 - f);
+
+  const double x = r_ecef.x;
+  const double y = r_ecef.y;
+  const double z = r_ecef.z;
+
+  const double lon = std::atan2(y, x);
+  const double p = std::sqrt(x * x + y * y);
+
+  if (p < 1e-9) {
+    const double lat = (z >= 0.0) ? (M_PI / 2.0) : (-M_PI / 2.0);
+    const double alt = std::abs(z) - a * std::sqrt(1.0 - e2);
+    return {lat, lon, alt};
+  }
+
+  double lat = std::atan2(z, p * (1.0 - e2));
+  double N = 0.0;
+  double alt = 0.0;
+
+  for (int k = 0; k < 6; ++k) {
+    const double s = std::sin(lat);
+    N = a / std::sqrt(1.0 - e2 * s * s);
+    alt = p / std::cos(lat) - N;
+    lat = std::atan2(z, p * (1.0 - e2 * (N / (N + alt))));
+  }
+
+  return {lat, lon, alt};
+}
+
+// Local NED basis written in ECEF components.
+// Each column of the matrix is one unit vector: [North East Down].
+inline Mat3 ned_basis_ecef(double lat_rad, double lon_rad) {
+  const double sphi = std::sin(lat_rad);
+  const double cphi = std::cos(lat_rad);
+  const double slam = std::sin(lon_rad);
+  const double clam = std::cos(lon_rad);
+
+  const Vec3 north{-sphi * clam, -sphi * slam,  cphi};
+  const Vec3 east {-slam,         clam,         0.0};
+  const Vec3 down {-cphi * clam, -cphi * slam, -sphi};
+
+  Mat3 A{};
+  A[0][0] = north.x; A[1][0] = north.y; A[2][0] = north.z;
+  A[0][1] = east.x;  A[1][1] = east.y;  A[2][1] = east.z;
+  A[0][2] = down.x;  A[1][2] = down.y;  A[2][2] = down.z;
+  return A;
+}

include/math.hpp

@@ -0,0 +1,157 @@
+#pragma once
+#include <array>
+#include <cmath>
+#include <stdexcept>
+
+// Simple 3D vector type used everywhere in the sim.
+// This is intentionally tiny so the math stays easy to follow.
+struct Vec3 {
+  double x{0.0};
+  double y{0.0};
+  double z{0.0};
+
+  Vec3() = default;
+  Vec3(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
+
+  // Allow v[0], v[1], v[2] access for loops.
+  double& operator[](size_t i) { return (i == 0) ? x : (i == 1) ? y : z; }
+  double  operator[](size_t i) const { return (i == 0) ? x : (i == 1) ? y : z; }
+};
+
+inline Vec3 operator+(const Vec3& a, const Vec3& b) { return {a.x + b.x, a.y + b.y, a.z + b.z}; }
+inline Vec3 operator-(const Vec3& a, const Vec3& b) { return {a.x - b.x, a.y - b.y, a.z - b.z}; }
+inline Vec3 operator*(double s, const Vec3& v) { return {s * v.x, s * v.y, s * v.z}; }
+inline Vec3 operator*(const Vec3& v, double s) { return s * v; }
+inline Vec3 operator/(const Vec3& v, double s) { return {v.x / s, v.y / s, v.z / s}; }
+
+inline double dot(const Vec3& a, const Vec3& b) {
+  return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+
+inline Vec3 cross(const Vec3& a, const Vec3& b) {
+  return {
+    a.y * b.z - a.z * b.y,
+    a.z * b.x - a.x * b.z,
+    a.x * b.y - a.y * b.x
+  };
+}
+
+inline double norm(const Vec3& v) {
+  return std::sqrt(dot(v, v));
+}
+
+inline Vec3 normalize(const Vec3& v) {
+  const double n = norm(v);
+  if (n == 0.0) {
+    return {0.0, 0.0, 0.0};
+  }
+  return v / n;
+}
+
+// 3x3 matrix stored row-by-row.
+struct Mat3 {
+  double m[3][3]{};
+  double* operator[](size_t r) { return m[r]; }
+  const double* operator[](size_t r) const { return m[r]; }
+};
+
+inline Mat3 eye3() {
+  Mat3 I{};
+  I[0][0] = 1.0;
+  I[1][1] = 1.0;
+  I[2][2] = 1.0;
+  return I;
+}
+
+inline Mat3 transpose(const Mat3& A) {
+  Mat3 T{};
+  for (int r = 0; r < 3; ++r) {
+    for (int c = 0; c < 3; ++c) {
+      T[r][c] = A[c][r];
+    }
+  }
+  return T;
+}
+
+inline Vec3 mul(const Mat3& A, const Vec3& v) {
+  return {
+    A[0][0] * v.x + A[0][1] * v.y + A[0][2] * v.z,
+    A[1][0] * v.x + A[1][1] * v.y + A[1][2] * v.z,
+    A[2][0] * v.x + A[2][1] * v.y + A[2][2] * v.z
+  };
+}
+
+inline Mat3 mul(const Mat3& A, const Mat3& B) {
+  Mat3 C{};
+  for (int r = 0; r < 3; ++r) {
+    for (int c = 0; c < 3; ++c) {
+      double s = 0.0;
+      for (int k = 0; k < 3; ++k) {
+        s += A[r][k] * B[k][c];
+      }
+      C[r][c] = s;
+    }
+  }
+  return C;
+}
+
+inline Mat3 add(const Mat3& A, const Mat3& B) {
+  Mat3 C{};
+  for (int r = 0; r < 3; ++r) {
+    for (int c = 0; c < 3; ++c) {
+      C[r][c] = A[r][c] + B[r][c];
+    }
+  }
+  return C;
+}
+
+inline Mat3 scale(const Mat3& A, double s) {
+  Mat3 C{};
+  for (int r = 0; r < 3; ++r) {
+    for (int c = 0; c < 3; ++c) {
+      C[r][c] = A[r][c] * s;
+    }
+  }
+  return C;
+}
+
+// Skew-symmetric matrix used for cross-product style formulas.
+inline Mat3 skew(const Vec3& v) {
+  Mat3 S{};
+  S[0][1] = -v.z;
+  S[0][2] =  v.y;
+  S[1][0] =  v.z;
+  S[1][2] = -v.x;
+  S[2][0] = -v.y;
+  S[2][1] =  v.x;
+  return S;
+}
+
+// Plain 3x3 inverse. This assumes the matrix really is invertible.
+inline Mat3 inv3(const Mat3& A) {
+  const double a = A[0][0], b = A[0][1], c = A[0][2];
+  const double d = A[1][0], e = A[1][1], f = A[1][2];
+  const double g = A[2][0], h = A[2][1], i = A[2][2];
+
+  const double A11 =  (e * i - f * h);
+  const double A12 = -(d * i - f * g);
+  const double A13 =  (d * h - e * g);
+  const double A21 = -(b * i - c * h);
+  const double A22 =  (a * i - c * g);
+  const double A23 = -(a * h - b * g);
+  const double A31 =  (b * f - c * e);
+  const double A32 = -(a * f - c * d);
+  const double A33 =  (a * e - b * d);
+
+  const double det = a * A11 + b * A12 + c * A13;
+  if (std::abs(det) < 1e-30) {
+    throw std::runtime_error("inv3: singular matrix");
+  }
+
+  const double invdet = 1.0 / det;
+  Mat3 inv{};
+  inv[0][0] = A11 * invdet; inv[0][1] = A21 * invdet; inv[0][2] = A31 * invdet;
+  inv[1][0] = A12 * invdet; inv[1][1] = A22 * invdet; inv[1][2] = A32 * invdet;
+  inv[2][0] = A13 * invdet; inv[2][1] = A23 * invdet; inv[2][2] = A33 * invdet;
+  return inv;
+}

include/modules.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include "sim_context.hpp"
+#include <array>
+
+// State vector layout used everywhere in the sim.
+// [x y z xdot ydot zdot q0 q1 q2 q3 p q r]
+using State13 = std::array<double, 13>;
+
+// Core rigid-body/orbit dynamics.
+State13 satellite_derivatives(double t, const State13& state, SimContext& ctx);
+
+// Sensor and navigation pieces.
+void sensor_update(Vec3& BB, Vec3& pqr, Vec3& ptp, SimContext& ctx);
+void navigation_update(const Vec3& BfieldMeasured, const Vec3& pqrMeasured, const Vec3& ptpMeasured, SimContext& ctx);
+
+// Magnetorquer controller.
+void control_compute(const Vec3& BfieldNav, const Vec3& pqrNav, const Vec3& ptpNav,
+                     SimContext& ctx, Vec3& current_out);
+
+// Disturbance model.
+void disturbance(double altitude_m, double Amax, double lmax, const Vec3& vel, double CD, const Vec3& BI_Tesla,
+                 Vec3& XYZD_out, Vec3& LMND_out);
+
+double density_kg_m3(double altitude_m);