diff --git a/.gitignore b/.gitignore index 3ad34c5..1e5d358 100644 --- a/.gitignore +++ b/.gitignore @@ -1,49 +1,46 @@ -# Prerequisites -*.d +# Python / virtual environments +.venv/ +__pycache__/ +**/__pycache__/ +*.pyc +.pytest_cache/ -# Compiled Object files -*.slo -*.lo -*.o +# CMake / binary build products +build/ +dist/ +dist3/ +CMakeFiles/ +CMakeCache.txt +cmake_install.cmake +*.exe *.obj - -# Precompiled Headers -*.gch -*.pch - -# Linker files -*.ilk - -# Debugger Files -*.pdb - -# Compiled Dynamic libraries -*.so -*.dylib +*.o +*.lib *.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la +*.pyd *.a -*.lib - -# Executables -*.exe -*.out -*.app - -# debug information files -*.dwo +*.so -# idk -build/ -scripts/.venv/ -*.pyc -__pycache__/ -output/ -.vscode/ \ No newline at end of file +# Generated simulation outputs +*.csv +*.png +*.npz +*.pkl +output_data/ +output_plots/ +basilisk_runner/output_data/ +basilisk_runner/output_plots/ +reference_standalone/adcs_output.csv +reference_standalone/output_plots/ + +# Local editor state; keep intentional VS Code tasks tracked +.vscode/* +!.vscode/tasks.json + +# Misc temporary files +*.tmp +*.log +.DS_Store +Thumbs.db + +archive/ diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..f960ecf --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,47 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "Build ADCS core", + "type": "shell", + "command": "cmake -S cpp_adcs_core -B build -DCMAKE_BUILD_TYPE=Release; cmake --build build --parallel", + "options": { "cwd": "${workspaceFolder}" }, + "problemMatcher": [] + }, + { + "label": "Run standalone reference", + "type": "shell", + "command": "cmake -S . -B build; cmake --build build; .\\build\\adcs.exe; python .\\plot_all.py", + "options": { "cwd": "${workspaceFolder}\\reference_standalone\\original_project" }, + "problemMatcher": [] + }, + { + "label": "Run Basilisk minimal", + "type": "shell", + "command": "python .\\basilisk_runner\\scenario_huskysat2_minimal.py", + "options": { "cwd": "${workspaceFolder}" }, + "problemMatcher": [] + }, + { + "label": "Run Basilisk detumble", + "type": "shell", + "command": "python .\\basilisk_runner\\scenario_huskysat2_detumble.py", + "options": { "cwd": "${workspaceFolder}" }, + "problemMatcher": [] + }, + { + "label": "Plot Basilisk results", + "type": "shell", + "command": "python .\\basilisk_runner\\plot_results.py", + "options": { "cwd": "${workspaceFolder}" }, + "problemMatcher": [] + }, + { + "label": "Compare reference vs Basilisk", + "type": "shell", + "command": "python .\\basilisk_runner\\compare_reference_vs_basilisk.py", + "options": { "cwd": "${workspaceFolder}" }, + "problemMatcher": [] + } + ] +} diff --git a/README.md b/README.md index 3f86197..7e33106 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,447 @@ -# HS2-ADCS_Sim -A simulation suite for the ADCS aboard HS2. +# HS2 ADCS Sim — Basilisk migration baseline + +This repository contains the HuskySat-2 ADCS simulation work and a Basilisk migration baseline. + +The main workflow goal is: + +```text +Use Basilisk as an installed Python dependency, not as a source tree that must be rebuilt for every controller edit. +``` + +Normal scenario/controller updates should run through Python scripts in `basilisk_runner/` and should not require rebuilding Basilisk or importing `Basilisk.ExternalModules`. + +## Current status + +Validated in the Basilisk migration baseline: + +- Fast pip-installed Basilisk workflow. +- Minimal Basilisk spacecraft propagation. +- Detumble scenario with magnetic field, body-rate, current, dipole, coil-power, applied-torque, Euler-angle, and summary plots. +- Detumble comparison against the standalone C++ reference using metric/invariant checks. +- Direct-torque pointing proof-of-concept with pointing-error, body-rate, torque, Euler-angle, and summary plots. +- Standalone C++ reference build fixed for Windows/MSVC. +- Math validation for detumble and pointing. + +Known follow-ups: + +- Native Basilisk `MtbEffector` wiring is not resolved yet. +- Magnetorquer-limited full-attitude pointing is not solved yet. +- Direct-torque pointing is a guidance/control proof-of-concept, not final flight actuator behavior. + +## Required software + +Install these before running the project on Windows: + +1. **Git** + - Required for cloning, branches, commits, and PR workflow. + +2. **Python** + - Use a Python version supported by the installed Basilisk wheel. + - The current workflow was tested with a project virtual environment and `bsk` installed from `requirements.txt`. + +3. **PowerShell** + - Commands below are written for PowerShell on Windows. + +4. **CMake** + - Required to build the preserved standalone C++ reference and optional `cpp_adcs_core/`. + +5. **Visual Studio 2022 Community or Build Tools for Visual Studio 2022** + - Required for MSVC C++ compilation on Windows. + - Make sure the **Desktop development with C++** workload is installed. + +6. **VS Code** *(optional but recommended)* + - Useful for editing, terminal workflow, and provided `.vscode/tasks.json` tasks. + +7. **Internet access** + - Needed for the first `pip install -r requirements.txt`. + +## Python dependencies + +Python dependencies are listed in: + +```text +requirements.txt +``` + +Current major dependencies include: + +```text +bsk[examples] +numpy +pandas +matplotlib +pytest +pybind11 +``` + +`bsk[examples]` installs Basilisk from PyPI. Do not clone or rebuild Basilisk for normal scenario/controller edits. + +## Fresh clone and setup + +Until the Basilisk migration PR is merged, use the PR branch: + +```powershell +git clone https://github.com/UWCubeSat/HS2-ADCS_Sim.git +cd HS2-ADCS_Sim +git switch basilisk-migration-baseline-v2 +``` + +After the PR is merged, use `main` instead: + +```powershell +git clone https://github.com/UWCubeSat/HS2-ADCS_Sim.git +cd HS2-ADCS_Sim +``` + +Create and activate the virtual environment: + +```powershell +python -m venv .venv +Set-ExecutionPolicy -Scope Process -ExecutionPolicy RemoteSigned +.\.venv\Scripts\Activate.ps1 +python -m pip install --upgrade pip +python -m pip install -r requirements.txt +``` + +Confirm Basilisk imports: + +```powershell +python -c "import Basilisk; print(Basilisk.__path__)" +python -c "from Basilisk.utilities import SimulationBaseClass, macros; from Basilisk.simulation import spacecraft, gravityEffector; print('Basilisk imports OK')" +``` + +## Repository layout + +```text +HS2-ADCS_Sim/ +├── basilisk_runner/ +│ ├── scenario_huskysat2_minimal.py +│ ├── scenario_huskysat2_detumble.py +│ ├── scenario_huskysat2_pointing.py +│ ├── basilisk_adcs_adapter.py +│ ├── plot_results.py +│ ├── compare_reference_vs_basilisk.py +│ ├── output_data/ # generated, ignored by git +│ └── output_plots/ # generated, ignored by git +├── cpp_adcs_core/ +│ ├── CMakeLists.txt +│ ├── include/adcs/adcs_core.hpp +│ ├── src/adcs_core.cpp +│ ├── tests/test_adcs_core.cpp +│ └── bindings/pybind11_module.cpp +├── reference_standalone/ +│ └── original_project/ # preserved standalone C++ reference source +├── docs/ +│ ├── source_inspection_report.md +│ ├── basilisk_capability_audit.md +│ ├── migration_plan.md +│ ├── interface_contract.md +│ ├── validation_plan.md +│ └── build_workflow.md +└── requirements.txt +``` + +## Run the minimal Basilisk scenario + +From the repository root with the virtual environment active: + +```powershell +python .\basilisk_runner\scenario_huskysat2_minimal.py +``` + +Expected behavior: + +- Writes `basilisk_runner/output_data/minimal_output.csv`. +- Prints initial and final angular speed. +- Angular speed should stay nearly constant because there is no active detumble controller in this minimal scenario. + +## Build and run the standalone C++ reference + +The large generated reference CSV is intentionally not committed to Git. To run detumble comparison, regenerate it locally. + +From the repository root: + +```powershell +cd .\reference_standalone\original_project +cmake -S . -B build +cmake --build build --config Release +.\build\Release\adcs.exe +python .\plot_all.py +Copy-Item .\adcs_output.csv ..\adcs_output.csv -Force +cd ..\.. +``` + +Expected outputs: + +```text +reference_standalone/original_project/adcs_output.csv +reference_standalone/adcs_output.csv +reference_standalone/original_project/output_plots/ +``` + +Do not commit generated CSVs or plots. + +## Run the Basilisk detumble scenario + +From the repository root with the virtual environment active: + +```powershell +python .\basilisk_runner\scenario_huskysat2_detumble.py +``` + +Expected generated files: + +```text +basilisk_runner/output_data/detumble_output.csv +basilisk_runner/output_plots/body_rates.png +basilisk_runner/output_plots/nav_euler.png +basilisk_runner/output_plots/mag_field.png +basilisk_runner/output_plots/currents.png +basilisk_runner/output_plots/dipole.png +basilisk_runner/output_plots/coil_power.png +basilisk_runner/output_plots/mtb_torque.png +basilisk_runner/output_plots/summary_plots.png +``` + +The current detumble baseline uses direct application of the computed magnetic torque through Basilisk body torque: + +```text +tau_B = m_B x B_B +``` + +This is a validated fallback path. Native Basilisk `MtbEffector` integration remains a follow-up task. + +## Compare standalone reference vs. Basilisk detumble + +First make sure the standalone reference CSV exists: + +```text +reference_standalone/adcs_output.csv +``` + +Then run: + +```powershell +python .\basilisk_runner\compare_reference_vs_basilisk.py +``` + +Expected terminal line: + +```text +Validation passed?: True +``` + +The comparison is metric-based, not CSV-column-exact. It checks: + +- finite numeric values, +- final angular speed less than initial angular speed, +- rotational kinetic energy decrease, +- mean mechanical control power is negative, +- `tau_B = m_B x B_B` consistency, +- magnetic-field magnitude plausibility, +- body/inertial magnetic-field norm preservation, +- peak coil-power limit, +- detumble time to `0.05 rad/s` within the expected window. + +Metrics are written to: + +```text +basilisk_runner/output_data/comparison_metrics.json +``` + +## Run the direct-torque pointing proof-of-concept + +From the repository root with the virtual environment active: + +```powershell +python .\basilisk_runner\scenario_huskysat2_pointing.py +``` + +Expected terminal line: + +```text +Pointing validation passed?: True +``` + +Expected generated files: + +```text +basilisk_runner/output_data/pointing_output.csv +basilisk_runner/output_data/pointing_metrics.json +basilisk_runner/output_plots/pointing_error.png +basilisk_runner/output_plots/pointing_body_rates.png +basilisk_runner/output_plots/pointing_torque.png +basilisk_runner/output_plots/pointing_euler.png +basilisk_runner/output_plots/pointing_summary.png +``` + +This scenario validates guidance/control behavior with an ideal direct body-torque actuator. It is not the final magnetorquer-only pointing controller. + +## Full local validation sequence + +Run this from the repository root after setting up the virtual environment. + +```powershell +# 1. Regenerate standalone reference CSV +cd .\reference_standalone\original_project +cmake -S . -B build +cmake --build build --config Release +.\build\Release\adcs.exe +Copy-Item .\adcs_output.csv ..\adcs_output.csv -Force +cd ..\.. + +# 2. Run Basilisk detumble and comparison +python .\basilisk_runner\scenario_huskysat2_detumble.py +python .\basilisk_runner\compare_reference_vs_basilisk.py + +# 3. Run pointing proof-of-concept validation +python .\basilisk_runner\scenario_huskysat2_pointing.py + +# 4. Confirm git tree is clean + git status +``` + +Expected validation lines: + +```text +Validation passed?: True +Pointing validation passed?: True +``` + +Expected Git status: + +```text +nothing to commit, working tree clean +``` + +## Plot existing detumble output + +If `detumble_output.csv` already exists, regenerate plots with: + +```powershell +python .\basilisk_runner\plot_results.py +``` + +## Optional: build the standalone ADCS C++ core + +Only do this when custom C++ controller logic changes: + +```powershell +cmake -S cpp_adcs_core -B build\cpp_adcs_core -DCMAKE_BUILD_TYPE=Release +cmake --build build\cpp_adcs_core --config Release --parallel +``` + +If CMake cannot find pybind11: + +```powershell +$pybindDir = python -m pybind11 --cmakedir +cmake -S cpp_adcs_core -B build\cpp_adcs_core -DCMAKE_BUILD_TYPE=Release -Dpybind11_DIR=$pybindDir +cmake --build build\cpp_adcs_core --config Release --parallel +``` + +This builds only the small project-owned C++ core. It does not rebuild Basilisk. + +## Normal development workflow + +For most changes: + +```text +1. Edit Python scenario/controller code in basilisk_runner/. +2. Run the relevant scenario. +3. Run validation scripts. +4. Inspect generated plots/metrics. +5. Commit source/docs/config changes only. +``` + +If C++ controller code changes: + +```text +1. Edit cpp_adcs_core/. +2. Rebuild cpp_adcs_core only. +3. Run Basilisk scenario and validation. +``` + +Do not rebuild Basilisk for normal work. + +## Files that should not be committed + +These are generated or local-only artifacts: + +```text +.venv/ +build/ +archive/ +basilisk_runner/output_data/ +basilisk_runner/output_plots/ +reference_standalone/adcs_output.csv +reference_standalone/output_plots/ +reference_standalone/original_project/build/ +reference_standalone/original_project/adcs_output.csv +reference_standalone/original_project/output_plots/ +``` + +## Troubleshooting + +### `Basilisk is not installed` + +Activate the virtual environment and install requirements: + +```powershell +.\.venv\Scripts\Activate.ps1 +python -m pip install -r requirements.txt +``` + +### `Reference CSV not found` + +Regenerate the standalone reference: + +```powershell +cd .\reference_standalone\original_project +cmake -S . -B build +cmake --build build --config Release +.\build\Release\adcs.exe +Copy-Item .\adcs_output.csv ..\adcs_output.csv -Force +cd ..\.. +``` + +Then rerun: + +```powershell +python .\basilisk_runner\compare_reference_vs_basilisk.py +``` + +### `.uild +dcs.exe is not recognized` or executable not found + +With the Visual Studio CMake generator, the executable is usually under the configuration folder: + +```powershell +.\build\Release\adcs.exe +``` + +not: + +```powershell +.\build\adcs.exe +``` + +### C++ build cannot find `M_PI` + +The standalone reference CMake file defines `_USE_MATH_DEFINES` for MSVC. If this error appears, confirm this line exists in `reference_standalone/original_project/CMakeLists.txt`: + +```cmake +target_compile_definitions(adcs PRIVATE _USE_MATH_DEFINES) +``` + +### Git shows generated files + +Generated output should be ignored. If generated files appear, do not commit them. Confirm `.gitignore` includes output folders and local archives. + +## Follow-up work + +Keep these out of the baseline PR unless explicitly requested: + +1. Magnetorquer-limited pointing controller. +2. Native Basilisk `MtbEffector` isolated debugging. +3. GitHub Actions / CI smoke testing. +4. More complete disturbance-model parity with the standalone reference. diff --git a/basilisk_runner/basilisk_adcs_adapter.py b/basilisk_runner/basilisk_adcs_adapter.py new file mode 100644 index 0000000..39e01a9 --- /dev/null +++ b/basilisk_runner/basilisk_adcs_adapter.py @@ -0,0 +1,191 @@ +"""Basilisk-to-ADCS adapter layer. + +This file intentionally does not import Basilisk.ExternalModules. It provides a +Python Basilisk SysModel for fast iteration and optionally calls a separately +built pybind module named ``adcs_core`` if it is importable. +""" + +from __future__ import annotations + +from dataclasses import dataclass, asdict +from typing import Dict, List, Sequence +import math +import numpy as np + +try: + import adcs_core as _adcs_core # optional pybind module from cpp_adcs_core +except Exception: # pragma: no cover - local optional dependency + _adcs_core = None + +from Basilisk.architecture import sysModel, messaging, bskLogging + + +MAX_EFF_CNT = 36 + + +@dataclass +class ADCSConfig: + dipoleCommandGain: float = 67200.0 + mtqDipoleGain_Am2_A: Sequence[float] = (2.3, 2.3, 0.85 / math.sqrt(1.75 / 4.4)) + mtqResistance_Ohm: Sequence[float] = (51.0, 51.0, 4.4) + mtqCurrentLimit_A: Sequence[float] = (5.0 / 51.0, 5.0 / 51.0, math.sqrt(1.75 / 4.4)) + mtqDipoleLimit_Am2: Sequence[float] = (0.2, 0.2, 0.85) + minMagField_T: float = 1.0e-12 + use_cpp_core_if_available: bool = True + + +def _cross(a: Sequence[float], b: Sequence[float]) -> np.ndarray: + return np.cross(np.asarray(a, dtype=float), np.asarray(b, dtype=float)) + + +def _controller_step_python(time_s: float, omega_B_rad_s: Sequence[float], mag_B_T: Sequence[float], cfg: ADCSConfig) -> Dict[str, object]: + omega = np.asarray(omega_B_rad_s, dtype=float) + mag = np.asarray(mag_B_T, dtype=float) + + result = { + "commanded_magnetic_dipole_B_Am2": [0.0, 0.0, 0.0], + "commanded_coil_current_A": [0.0, 0.0, 0.0], + "commanded_control_torque_B_Nm": [0.0, 0.0, 0.0], + "coil_power_W": [0.0, 0.0, 0.0], + "coil_power_total_W": 0.0, + "saturation_flags": [False, False, False], + "valid": True, + } + + if not (np.isfinite(time_s) and np.all(np.isfinite(omega)) and np.all(np.isfinite(mag))): + result["valid"] = False + return result + + if float(np.linalg.norm(mag)) < cfg.minMagField_T: + result["valid"] = False + return result + + desired_dipole = cfg.dipoleCommandGain * _cross(omega, mag) + gains = np.asarray(cfg.mtqDipoleGain_Am2_A, dtype=float) + resist = np.asarray(cfg.mtqResistance_Ohm, dtype=float) + current_limits = np.asarray(cfg.mtqCurrentLimit_A, dtype=float) + dipole_limits = np.asarray(cfg.mtqDipoleLimit_Am2, dtype=float) + + currents = np.zeros(3) + dipoles = np.zeros(3) + powers = np.zeros(3) + flags = [False, False, False] + + for i in range(3): + if gains[i] <= 0.0 or current_limits[i] <= 0.0 or dipole_limits[i] <= 0.0 or resist[i] < 0.0: + result["valid"] = False + flags[i] = True + continue + allowed_current = min(current_limits[i], dipole_limits[i] / gains[i]) + raw_current = desired_dipole[i] / gains[i] + cmd_current = float(np.clip(raw_current, -allowed_current, allowed_current)) + currents[i] = cmd_current + dipoles[i] = cmd_current * gains[i] + powers[i] = resist[i] * cmd_current * cmd_current + flags[i] = abs(raw_current - cmd_current) > 1.0e-15 + + torque = _cross(dipoles, mag) + result.update({ + "commanded_magnetic_dipole_B_Am2": dipoles.tolist(), + "commanded_coil_current_A": currents.tolist(), + "commanded_control_torque_B_Nm": torque.tolist(), + "coil_power_W": powers.tolist(), + "coil_power_total_W": float(np.sum(powers)), + "saturation_flags": flags, + }) + return result + + +def controller_step(time_s: float, omega_B_rad_s: Sequence[float], mag_B_T: Sequence[float], cfg: ADCSConfig | None = None) -> Dict[str, object]: + cfg = cfg or ADCSConfig() + if cfg.use_cpp_core_if_available and _adcs_core is not None: + try: + return _adcs_core.step(float(time_s), list(omega_B_rad_s), list(mag_B_T), asdict(cfg)) + except Exception: + # Fall back to Python so scenario bring-up is not blocked by pybind packaging. + pass + return _controller_step_python(time_s, omega_B_rad_s, mag_B_T, cfg) + + +class PythonBdotMTQController(sysModel.SysModel): + """Fast-prototyping Basilisk Python module for magnetorquer detumble. + + Inputs: + navAttInMsg: NavAttMsgPayload, usually from simpleNav + tamSensorInMsg: TAMSensorMsgPayload, magnetometer output; assumes S == B + + Output: + mtbCmdOutMsg: MTBCmdMsgPayload, first 3 dipoles populated + """ + + def __init__(self, config: ADCSConfig | None = None, *args): + super().__init__(*args) + self.config = config or ADCSConfig() + self.navAttInMsg = messaging.NavAttMsgReader() + self.tamSensorInMsg = messaging.TAMSensorMsgReader() + self.mtbCmdOutMsg = messaging.MTBCmdMsg() + self.cmdTorqueOutMsg = messaging.CmdTorqueBodyMsg() + self.history: List[Dict[str, object]] = [] + + def Reset(self, CurrentSimNanos): + if not self.navAttInMsg.isLinked(): + self.bskLogger.bskLog(bskLogging.BSK_ERROR, "PythonBdotMTQController.navAttInMsg is not linked.") + if not self.tamSensorInMsg.isLinked(): + self.bskLogger.bskLog(bskLogging.BSK_ERROR, "PythonBdotMTQController.tamSensorInMsg is not linked.") + payload = self.mtbCmdOutMsg.zeroMsgPayload + payload.mtbDipoleCmds = [0.0] * MAX_EFF_CNT + self.mtbCmdOutMsg.write(payload, CurrentSimNanos, self.moduleID) + torque_payload = self.cmdTorqueOutMsg.zeroMsgPayload + torque_payload.torqueRequestBody = [0.0, 0.0, 0.0] + self.cmdTorqueOutMsg.write(torque_payload, CurrentSimNanos, self.moduleID) + self.history.clear() + + def UpdateState(self, CurrentSimNanos): + nav = self.navAttInMsg() + tam = self.tamSensorInMsg() + + time_s = CurrentSimNanos * 1.0e-9 + omega = np.asarray(nav.omega_BN_B, dtype=float)[:3] + mag_B = np.asarray(tam.tam_S, dtype=float)[:3] + + cmd = controller_step(time_s, omega, mag_B, self.config) + + payload = self.mtbCmdOutMsg.zeroMsgPayload + dipoles3 = list(cmd["commanded_magnetic_dipole_B_Am2"]) + payload.mtbDipoleCmds = [0.0] * MAX_EFF_CNT + payload.mtbDipoleCmds[0:3] = dipoles3 + self.mtbCmdOutMsg.write(payload, CurrentSimNanos, self.moduleID) + + torque3 = list(cmd["commanded_control_torque_B_Nm"]) + torque_payload = self.cmdTorqueOutMsg.zeroMsgPayload + torque_payload.torqueRequestBody = torque3 + self.cmdTorqueOutMsg.write(torque_payload, CurrentSimNanos, self.moduleID) + + self.history.append({ + "time_s": time_s, + "omega_B_x_rad_s": float(omega[0]), + "omega_B_y_rad_s": float(omega[1]), + "omega_B_z_rad_s": float(omega[2]), + "B_B_x_T": float(mag_B[0]), + "B_B_y_T": float(mag_B[1]), + "B_B_z_T": float(mag_B[2]), + "mcmd_x_Am2": float(dipoles3[0]), + "mcmd_y_Am2": float(dipoles3[1]), + "mcmd_z_Am2": float(dipoles3[2]), + "ix_A": float(cmd["commanded_coil_current_A"][0]), + "iy_A": float(cmd["commanded_coil_current_A"][1]), + "iz_A": float(cmd["commanded_coil_current_A"][2]), + "pcoil_x_W": float(cmd["coil_power_W"][0]), + "pcoil_y_W": float(cmd["coil_power_W"][1]), + "pcoil_z_W": float(cmd["coil_power_W"][2]), + "pcoil_total_W": float(cmd["coil_power_total_W"]), + "control_torque_B_x_Nm": float(torque3[0]), + "control_torque_B_y_Nm": float(torque3[1]), + "control_torque_B_z_Nm": float(torque3[2]), + "control_torque_B_mag_Nm": float(np.linalg.norm(torque3)), + "saturation_x": bool(cmd["saturation_flags"][0]), + "saturation_y": bool(cmd["saturation_flags"][1]), + "saturation_z": bool(cmd["saturation_flags"][2]), + "controller_valid": bool(cmd["valid"]), + "using_cpp_core": bool(_adcs_core is not None and self.config.use_cpp_core_if_available), + }) diff --git a/basilisk_runner/compare_reference_vs_basilisk.py b/basilisk_runner/compare_reference_vs_basilisk.py new file mode 100644 index 0000000..db1ccfc --- /dev/null +++ b/basilisk_runner/compare_reference_vs_basilisk.py @@ -0,0 +1,277 @@ +from __future__ import annotations + +from pathlib import Path +import json +from typing import Any + +import numpy as np +import pandas as pd + +HERE = Path(__file__).resolve().parent +ROOT = HERE.parent +REF = ROOT / "reference_standalone" / "adcs_output.csv" +OUT_DATA = HERE / "output_data" +COMPARE_OUT = OUT_DATA / "comparison_metrics.json" + +# Must match the current HuskySat-style baseline used by the Basilisk scenarios. +# TODO: replace with verified HuskySat-2 inertia once the final vehicle properties are available. +MASS_KG = 2.6 +LX_M, LY_M, LZ_M = 0.10, 0.10, 0.20 +IXX = (MASS_KG / 12.0) * (LY_M**2 + LZ_M**2) +IYY = (MASS_KG / 12.0) * (LX_M**2 + LZ_M**2) +IZZ = (MASS_KG / 12.0) * (LX_M**2 + LY_M**2) +INERTIA_DIAG_KG_M2 = np.array([IXX, IYY, IZZ], dtype=float) + + +def mag3(df: pd.DataFrame, cols: list[str]) -> np.ndarray: + return np.sqrt(sum(np.asarray(df[c], dtype=float) ** 2 for c in cols)) + + +def vec3(df: pd.DataFrame, cols: list[str]) -> np.ndarray: + return np.column_stack([np.asarray(df[c], dtype=float) for c in cols]) + + +def detumble_time(t: np.ndarray, wmag: np.ndarray, threshold: float) -> float | None: + idx = np.where(wmag <= threshold)[0] + return None if len(idx) == 0 else float(t[idx[0]]) + + +def finite_numeric(df: pd.DataFrame) -> bool: + numeric = df.select_dtypes(include=[np.number]) + return bool(np.isfinite(numeric.to_numpy(dtype=float)).all()) + + +def rotational_energy_J(omega_B_rad_s: np.ndarray) -> np.ndarray: + """Rigid-body rotational kinetic energy using the scenario diagonal inertia.""" + return 0.5 * np.sum((omega_B_rad_s**2) * INERTIA_DIAG_KG_M2.reshape((1, 3)), axis=1) + + +def maybe_remove_stale_comparison() -> None: + """Avoid leaving stale comparison_metrics.json after a failed compare run.""" + try: + COMPARE_OUT.unlink(missing_ok=True) + except TypeError: # Python < 3.8 defensive fallback + if COMPARE_OUT.exists(): + COMPARE_OUT.unlink() + + +def summarize_reference(df: pd.DataFrame) -> dict[str, Any]: + t = df["t"].to_numpy(dtype=float) + omega = vec3(df, ["p", "q", "r"]) + wmag = np.linalg.norm(omega, axis=1) + energy = rotational_energy_J(omega) + out: dict[str, Any] = { + "rows": int(len(df)), + "cols": int(len(df.columns)), + "finite_numeric": finite_numeric(df), + "initial_omega_mag_rad_s": float(wmag[0]), + "final_omega_mag_rad_s": float(wmag[-1]), + "detumble_time_0p05_s": detumble_time(t, wmag, 0.05), + "detumble_time_0p02_s": detumble_time(t, wmag, 0.02), + "detumble_time_0p01_s": detumble_time(t, wmag, 0.01), + "initial_rotational_energy_J": float(energy[0]), + "final_rotational_energy_J": float(energy[-1]), + "rotational_energy_reduction_fraction": float((energy[0] - energy[-1]) / energy[0]) if energy[0] > 0 else None, + } + if "pcoil_total" in df.columns: + out["mean_coil_power_W"] = float(df["pcoil_total"].mean()) + out["peak_coil_power_W"] = float(df["pcoil_total"].max()) + if all(c in df.columns for c in ["BBx", "BBy", "BBz"]): + bmag = mag3(df, ["BBx", "BBy", "BBz"]) + out["mean_B_body_T"] = float(bmag.mean()) + out["peak_B_body_T"] = float(bmag.max()) + if all(c in df.columns for c in ["Tdist_x", "Tdist_y", "Tdist_z"]): + tdist = mag3(df, ["Tdist_x", "Tdist_y", "Tdist_z"]) + out["mean_disturbance_torque_Nm"] = float(tdist.mean()) + return out + + +def summarize_basilisk(df: pd.DataFrame) -> dict[str, Any]: + t = df["time_s"].to_numpy(dtype=float) + omega = vec3(df, ["omega_B_x_rad_s", "omega_B_y_rad_s", "omega_B_z_rad_s"]) + if "omega_mag_rad_s" in df.columns: + wmag = df["omega_mag_rad_s"].to_numpy(dtype=float) + else: + wmag = np.linalg.norm(omega, axis=1) + + energy = rotational_energy_J(omega) + out: dict[str, Any] = { + "rows": int(len(df)), + "cols": int(len(df.columns)), + "finite_numeric": finite_numeric(df), + "initial_omega_mag_rad_s": float(wmag[0]), + "final_omega_mag_rad_s": float(wmag[-1]), + "detumble_time_0p05_s": detumble_time(t, wmag, 0.05), + "detumble_time_0p02_s": detumble_time(t, wmag, 0.02), + "detumble_time_0p01_s": detumble_time(t, wmag, 0.01), + "initial_rotational_energy_J": float(energy[0]), + "final_rotational_energy_J": float(energy[-1]), + "rotational_energy_reduction_fraction": float((energy[0] - energy[-1]) / energy[0]) if energy[0] > 0 else None, + } + if "pcoil_total_W" in df.columns: + out["mean_coil_power_W"] = float(df["pcoil_total_W"].mean()) + out["peak_coil_power_W"] = float(df["pcoil_total_W"].max()) + if "B_B_mag_T" in df.columns: + out["mean_B_body_T"] = float(df["B_B_mag_T"].mean()) + out["peak_B_body_T"] = float(df["B_B_mag_T"].max()) + elif all(c in df.columns for c in ["B_B_x_T", "B_B_y_T", "B_B_z_T"]): + bmag = mag3(df, ["B_B_x_T", "B_B_y_T", "B_B_z_T"]) + out["mean_B_body_T"] = float(bmag.mean()) + out["peak_B_body_T"] = float(bmag.max()) + + torque_cols = None + if all(c in df.columns for c in ["applied_torque_B_x_Nm", "applied_torque_B_y_Nm", "applied_torque_B_z_Nm"]): + torque_cols = ["applied_torque_B_x_Nm", "applied_torque_B_y_Nm", "applied_torque_B_z_Nm"] + torque_label = "applied_magnetic_torque" + elif all(c in df.columns for c in ["mtb_torque_B_x_Nm", "mtb_torque_B_y_Nm", "mtb_torque_B_z_Nm"]): + torque_cols = ["mtb_torque_B_x_Nm", "mtb_torque_B_y_Nm", "mtb_torque_B_z_Nm"] + torque_label = "mtb_torque" + else: + torque_label = "none" + + if torque_cols is not None: + torque = vec3(df, torque_cols) + torque_mag = np.linalg.norm(torque, axis=1) + mech_power = np.sum(omega * torque, axis=1) + nonzero_torque = torque_mag > 1e-14 + out[f"mean_{torque_label}_Nm"] = float(torque_mag.mean()) + out[f"peak_{torque_label}_Nm"] = float(torque_mag.max()) + out["mean_mechanical_control_power_W"] = float(mech_power.mean()) + out["min_mechanical_control_power_W"] = float(mech_power.min()) + out["max_mechanical_control_power_W"] = float(mech_power.max()) + out["negative_mechanical_power_fraction"] = float(np.mean(mech_power[nonzero_torque] < 0.0)) if np.any(nonzero_torque) else None + + if all(c in df.columns for c in ["mcmd_x_Am2", "mcmd_y_Am2", "mcmd_z_Am2", "B_B_x_T", "B_B_y_T", "B_B_z_T"]): + m_B = vec3(df, ["mcmd_x_Am2", "mcmd_y_Am2", "mcmd_z_Am2"]) + B_B = vec3(df, ["B_B_x_T", "B_B_y_T", "B_B_z_T"]) + tau_cross = np.cross(m_B, B_B) + if torque_cols is not None: + torque = vec3(df, torque_cols) + err = torque - tau_cross + err_mag = np.linalg.norm(err, axis=1) + tau_mag = np.linalg.norm(torque, axis=1) + out["torque_cross_product_mean_abs_error_Nm"] = float(err_mag.mean()) + out["torque_cross_product_max_abs_error_Nm"] = float(err_mag.max()) + out["torque_cross_product_max_rel_error"] = float(err_mag.max() / max(float(tau_mag.max()), 1e-30)) + + if all(c in df.columns for c in ["B_N_x_T", "B_N_y_T", "B_N_z_T", "B_B_x_T", "B_B_y_T", "B_B_z_T"]): + B_N = vec3(df, ["B_N_x_T", "B_N_y_T", "B_N_z_T"]) + B_B = vec3(df, ["B_B_x_T", "B_B_y_T", "B_B_z_T"]) + b_norm_err = np.abs(np.linalg.norm(B_N, axis=1) - np.linalg.norm(B_B, axis=1)) + out["max_B_frame_norm_error_T"] = float(b_norm_err.max()) + out["mean_B_frame_norm_error_T"] = float(b_norm_err.mean()) + + return out + + +def check(name: str, passed: bool, value: Any = None, limit: Any = None) -> dict[str, Any]: + return {"passed": bool(passed), "value": value, "limit": limit} + + +def build_validation(ref: dict[str, Any], bsk: dict[str, Any]) -> dict[str, Any]: + checks: dict[str, Any] = {} + checks["reference_numeric_finite"] = check("reference_numeric_finite", ref.get("finite_numeric") is True) + checks["basilisk_numeric_finite"] = check("basilisk_numeric_finite", bsk.get("finite_numeric") is True) + checks["basilisk_final_omega_less_than_initial"] = check( + "basilisk_final_omega_less_than_initial", + bsk["final_omega_mag_rad_s"] < bsk["initial_omega_mag_rad_s"], + bsk["final_omega_mag_rad_s"], + f"< {bsk['initial_omega_mag_rad_s']}", + ) + checks["basilisk_final_energy_less_than_initial"] = check( + "basilisk_final_energy_less_than_initial", + bsk["final_rotational_energy_J"] < bsk["initial_rotational_energy_J"], + bsk["final_rotational_energy_J"], + f"< {bsk['initial_rotational_energy_J']}", + ) + checks["basilisk_mean_mechanical_power_negative"] = check( + "basilisk_mean_mechanical_power_negative", + bsk.get("mean_mechanical_control_power_W", 1.0) < 0.0, + bsk.get("mean_mechanical_control_power_W"), + "< 0 W", + ) + checks["torque_matches_m_cross_B"] = check( + "torque_matches_m_cross_B", + bsk.get("torque_cross_product_max_abs_error_Nm", float("inf")) < 5e-8 + or bsk.get("torque_cross_product_max_rel_error", float("inf")) < 0.05, + { + "max_abs_error_Nm": bsk.get("torque_cross_product_max_abs_error_Nm"), + "max_rel_error": bsk.get("torque_cross_product_max_rel_error"), + }, + "max abs < 5e-8 Nm OR max rel < 5%", + ) + checks["B_body_mean_LEO_plausible"] = check( + "B_body_mean_LEO_plausible", + 2.0e-5 <= bsk.get("mean_B_body_T", 0.0) <= 7.0e-5, + bsk.get("mean_B_body_T"), + "2e-5 T <= mean <= 7e-5 T", + ) + checks["B_frame_norm_preserved"] = check( + "B_frame_norm_preserved", + bsk.get("max_B_frame_norm_error_T", float("inf")) < 1e-10, + bsk.get("max_B_frame_norm_error_T"), + "< 1e-10 T", + ) + checks["peak_coil_power_within_expected_limit"] = check( + "peak_coil_power_within_expected_limit", + bsk.get("peak_coil_power_W", float("inf")) <= 2.6, + bsk.get("peak_coil_power_W"), + "<= 2.6 W", + ) + checks["detumble_to_0p05_within_expected_window"] = check( + "detumble_to_0p05_within_expected_window", + bsk.get("detumble_time_0p05_s") is not None and 3000.0 <= bsk["detumble_time_0p05_s"] <= 3500.0, + bsk.get("detumble_time_0p05_s"), + "3000 s to 3500 s", + ) + return { + "passed": all(item["passed"] for item in checks.values()), + "checks": checks, + } + + +def main() -> None: + if not REF.exists(): + maybe_remove_stale_comparison() + raise SystemExit(f"Reference CSV not found: {REF}") + bsk_csv = OUT_DATA / "detumble_output.csv" + if not bsk_csv.exists(): + bsk_csv = OUT_DATA / "minimal_output.csv" + if not bsk_csv.exists(): + maybe_remove_stale_comparison() + raise SystemExit("No Basilisk output found. Run scenario_huskysat2_detumble.py or scenario_huskysat2_minimal.py first.") + + ref_df = pd.read_csv(REF) + bsk_df = pd.read_csv(bsk_csv) + ref = summarize_reference(ref_df) + bsk = summarize_basilisk(bsk_df) + validation = build_validation(ref, bsk) + + comparison = { + "reference_file": str(REF), + "basilisk_file": str(bsk_csv), + "reference": ref, + "basilisk": bsk, + "validation": validation, + "notes": [ + "This comparison is physical-metric based, not CSV schema exact.", + "The validation checks are range/invariant tests, not claims of exact standalone equivalence.", + "Exact agreement is not expected until WMM epoch, frames, controller timing, disturbances, and actuator models are aligned.", + ], + } + + COMPARE_OUT.parent.mkdir(parents=True, exist_ok=True) + COMPARE_OUT.write_text(json.dumps(comparison, indent=2), encoding="utf-8") + + print(json.dumps(comparison, indent=2)) + print(f"Wrote {COMPARE_OUT}") + print(f"Validation passed?: {validation['passed']}") + if not validation["passed"]: + failed = [name for name, item in validation["checks"].items() if not item["passed"]] + print("Failed checks:") + for name in failed: + print(f" - {name}") + + +if __name__ == "__main__": + main() diff --git a/basilisk_runner/plot_results.py b/basilisk_runner/plot_results.py new file mode 100644 index 0000000..00dd33c --- /dev/null +++ b/basilisk_runner/plot_results.py @@ -0,0 +1,165 @@ +from __future__ import annotations + +from pathlib import Path +import sys + +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + +HERE = Path(__file__).resolve().parent +OUT_DATA = HERE / "output_data" +OUT_PLOTS = HERE / "output_plots" +OUT_PLOTS.mkdir(parents=True, exist_ok=True) + + +def _save(name: str, output_dir: Path): + path = output_dir / name + plt.tight_layout() + plt.savefig(path, dpi=180) + plt.close() + print(f"Saved {path}") + return path + + +def _mag_cols(df: pd.DataFrame, cols): + return np.sqrt(sum(np.asarray(df[c], dtype=float) ** 2 for c in cols)) + + +def plot_all(df: pd.DataFrame | None = None, output_dir: Path = OUT_PLOTS): + output_dir.mkdir(parents=True, exist_ok=True) + if df is None: + preferred = OUT_DATA / "detumble_output.csv" + fallback = OUT_DATA / "minimal_output.csv" + source = preferred if preferred.exists() else fallback + if not source.exists(): + raise SystemExit("No Basilisk output found. Run a scenario first.") + df = pd.read_csv(source) + print(f"Loaded {source}") + + t = np.asarray(df["time_s"], dtype=float) + paths = [] + + rate_cols = ["omega_B_x_rad_s", "omega_B_y_rad_s", "omega_B_z_rad_s"] + if all(c in df.columns for c in rate_cols): + omega_mag = df["omega_mag_rad_s"].to_numpy() if "omega_mag_rad_s" in df else _mag_cols(df, rate_cols) + plt.figure(figsize=(9, 4)) + plt.plot(t, df[rate_cols[0]], label="p") + plt.plot(t, df[rate_cols[1]], label="q") + plt.plot(t, df[rate_cols[2]], label="r") + plt.plot(t, omega_mag, label="|omega|", linewidth=2) + plt.xlabel("Time [s]") + plt.ylabel("Rate [rad/s]") + plt.title("Body Rates") + plt.grid(True) + plt.legend() + paths.append(_save("body_rates.png", output_dir)) + + euler_cols = ["euler321_phi_rad", "euler321_theta_rad", "euler321_psi_rad"] + if all(c in df.columns for c in euler_cols): + plt.figure(figsize=(9, 4)) + plt.plot(t, df[euler_cols[0]], label="phi") + plt.plot(t, df[euler_cols[1]], label="theta") + plt.plot(t, df[euler_cols[2]], label="psi") + plt.xlabel("Time [s]") + plt.ylabel("Angle [rad]") + plt.title("Euler 321 Angles") + plt.grid(True) + plt.legend() + paths.append(_save("nav_euler.png", output_dir)) + + if "B_B_mag_T" in df.columns or all(c in df.columns for c in ["B_B_x_T", "B_B_y_T", "B_B_z_T"]): + B_body = df["B_B_mag_T"].to_numpy() if "B_B_mag_T" in df else _mag_cols(df, ["B_B_x_T", "B_B_y_T", "B_B_z_T"]) + plt.figure(figsize=(9, 4)) + plt.plot(t, B_body, label="|B| body/sensor") + if "B_N_mag_T" in df.columns: + plt.plot(t, df["B_N_mag_T"], label="|B| inertial", alpha=0.75) + plt.xlabel("Time [s]") + plt.ylabel("|B| [T]") + plt.title("Magnetic Field Magnitude") + plt.grid(True) + plt.legend() + paths.append(_save("mag_field.png", output_dir)) + + if all(c in df.columns for c in ["ix_A", "iy_A", "iz_A"]): + plt.figure(figsize=(9, 4)) + plt.plot(t, df["ix_A"], label="ix") + plt.plot(t, df["iy_A"], label="iy") + plt.plot(t, df["iz_A"], label="iz") + plt.xlabel("Time [s]") + plt.ylabel("Current [A]") + plt.title("Magnetorquer Currents") + plt.grid(True) + plt.legend() + paths.append(_save("currents.png", output_dir)) + + if all(c in df.columns for c in ["mcmd_x_Am2", "mcmd_y_Am2", "mcmd_z_Am2"]): + plt.figure(figsize=(9, 4)) + plt.plot(t, df["mcmd_x_Am2"], label="mx") + plt.plot(t, df["mcmd_y_Am2"], label="my") + plt.plot(t, df["mcmd_z_Am2"], label="mz") + plt.xlabel("Time [s]") + plt.ylabel("Dipole [A m^2]") + plt.title("Magnetorquer Dipole Commands") + plt.grid(True) + plt.legend() + paths.append(_save("dipole.png", output_dir)) + + if "pcoil_total_W" in df.columns: + plt.figure(figsize=(9, 4)) + for c, label in [("pcoil_x_W", "Px"), ("pcoil_y_W", "Py"), ("pcoil_z_W", "Pz")]: + if c in df.columns: + plt.plot(t, df[c], label=label) + plt.plot(t, df["pcoil_total_W"], label="P total", linewidth=2) + plt.xlabel("Time [s]") + plt.ylabel("Power [W]") + plt.title("Coil Power") + plt.grid(True) + plt.legend() + paths.append(_save("coil_power.png", output_dir)) + + mtb_torque_cols = ["mtb_torque_B_x_Nm", "mtb_torque_B_y_Nm", "mtb_torque_B_z_Nm"] + if all(c in df.columns for c in mtb_torque_cols): + mtb_torque_mag = df["mtb_torque_B_mag_Nm"].to_numpy() if "mtb_torque_B_mag_Nm" in df else _mag_cols(df, mtb_torque_cols) + plt.figure(figsize=(9, 4)) + plt.plot(t, df[mtb_torque_cols[0]], label="Tx") + plt.plot(t, df[mtb_torque_cols[1]], label="Ty") + plt.plot(t, df[mtb_torque_cols[2]], label="Tz") + plt.plot(t, mtb_torque_mag, label="|T|", linewidth=2) + plt.xlabel("Time [s]") + plt.ylabel("Torque [N m]") + plt.title("Magnetorquer Applied Torque") + plt.grid(True) + plt.legend() + paths.append(_save("mtb_torque.png", output_dir)) + + if paths: + ncols = 2 + nrows = int(np.ceil(len(paths) / ncols)) + fig, axes = plt.subplots(nrows, ncols, figsize=(14, 4.8 * nrows)) + axes = np.atleast_1d(axes).ravel() + for ax, path in zip(axes, paths): + ax.imshow(plt.imread(path)) + ax.set_title(path.stem.replace("_", " ").title()) + ax.axis("off") + for ax in axes[len(paths):]: + ax.axis("off") + paths.append(_save("summary_plots.png", output_dir)) + + numeric = df.select_dtypes(include=[np.number]) + print("Rows:", len(df), "Cols:", len(df.columns)) + print("All numeric values finite?:", bool(np.isfinite(numeric.to_numpy(dtype=float)).all())) + if "omega_mag_rad_s" in df.columns: + print("Initial |omega| [rad/s]:", float(df["omega_mag_rad_s"].iloc[0])) + print("Final |omega| [rad/s]:", float(df["omega_mag_rad_s"].iloc[-1])) + if "pcoil_total_W" in df.columns: + print("Mean coil power [W]:", float(df["pcoil_total_W"].mean())) + print("Peak coil power [W]:", float(df["pcoil_total_W"].max())) + if "mtb_torque_B_mag_Nm" in df.columns: + print("Mean |MTB torque| [N m]:", float(df["mtb_torque_B_mag_Nm"].mean())) + print("Peak |MTB torque| [N m]:", float(df["mtb_torque_B_mag_Nm"].max())) + return paths + + +if __name__ == "__main__": + plot_all() diff --git a/basilisk_runner/scenario_huskysat2_detumble.py b/basilisk_runner/scenario_huskysat2_detumble.py new file mode 100644 index 0000000..ca16025 --- /dev/null +++ b/basilisk_runner/scenario_huskysat2_detumble.py @@ -0,0 +1,362 @@ +"""HuskySat-style Basilisk WMM + magnetorquer detumble scenario. + +This is the fast workflow path: +- pip-installed Basilisk +- no Basilisk source rebuild +- no Basilisk.ExternalModules import +- Python Basilisk controller module with optional separate pybind core +""" + +from __future__ import annotations + +from pathlib import Path +import math + +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + +try: + import Basilisk + from Basilisk.utilities import SimulationBaseClass, macros + from Basilisk.simulation import spacecraft, gravityEffector, magneticFieldWMM, simpleNav, magnetometer, MtbEffector, extForceTorque + from Basilisk.architecture import messaging +except ImportError as exc: + raise SystemExit( + "Basilisk is not installed in this Python environment. Run: python -m pip install -r requirements.txt" + ) from exc + +from basilisk_adcs_adapter import ADCSConfig, PythonBdotMTQController, MAX_EFF_CNT + +HERE = Path(__file__).resolve().parent +ROOT = HERE.parent +OUT_DATA = HERE / "output_data" +OUT_PLOTS = HERE / "output_plots" +OUT_DATA.mkdir(parents=True, exist_ok=True) +OUT_PLOTS.mkdir(parents=True, exist_ok=True) + +MASS_KG = 2.6 +LX_M, LY_M, LZ_M = 0.10, 0.10, 0.20 +IXX = (MASS_KG / 12.0) * (LY_M**2 + LZ_M**2) +IYY = (MASS_KG / 12.0) * (LX_M**2 + LZ_M**2) +IZZ = (MASS_KG / 12.0) * (LX_M**2 + LY_M**2) +MU_EARTH = 3.986004418e14 +EARTH_RADIUS_M = 6.371e6 +ALTITUDE_M = 600e3 +INCLINATION_DEG = 56.0 +INITIAL_RATES = [0.8, -0.2, 0.3] +CONTROL_DT_S = 0.1 +RECORD_DT_S = 1.0 +MAG_NOISE_STD_T = 0.0 +EPOCH_FRACTIONAL_YEAR = 2026.0 +USE_DIRECT_TORQUE_FALLBACK = True # Directly applies m x B through Basilisk extForceTorque while MtbEffector wiring is investigated. + + +def find_wmm2025_path() -> str: + """Find WMM2025.COF without hard-coding a user-specific Basilisk build path.""" + candidates = [] + for base in [Path(Basilisk.__path__[0]), ROOT / "reference_standalone" / "original_project"]: + if base.exists(): + candidates.extend(base.glob("**/WMM2025.COF")) + if candidates: + return str(candidates[0]) + raise FileNotFoundError( + "Could not find WMM2025.COF. Install Basilisk support data or keep the standalone reference file at " + "reference_standalone/original_project/reference/WMM2025.COF." + ) + + +def mrp_to_dcm(sigma): + """Convert an MRP attitude vector to the associated direction cosine matrix. + + This is used only for diagnostic Euler-angle output. Basilisk internally + propagates MRPs, which remain the preferred attitude representation for + simulation and control. + """ + s = np.asarray(sigma, dtype=float) + s2 = float(np.dot(s, s)) + sx = np.array([ + [0.0, -s[2], s[1]], + [s[2], 0.0, -s[0]], + [-s[1], s[0], 0.0], + ]) + return np.eye(3) + (8.0 * sx @ sx - 4.0 * (1.0 - s2) * sx) / (1.0 + s2) ** 2 + + +def dcm_to_euler321(C): + """Return 3-2-1 Euler angles [phi, theta, psi] from a DCM.""" + C = np.asarray(C, dtype=float) + phi = math.atan2(C[1, 2], C[2, 2]) + theta = -math.asin(float(np.clip(C[0, 2], -1.0, 1.0))) + psi = math.atan2(C[0, 1], C[0, 0]) + return np.array([phi, theta, psi]) + + +def configure_spacecraft(): + sc = spacecraft.Spacecraft() + sc.ModelTag = "HuskySat2_detumble" + sc.hub.mHub = MASS_KG + sc.hub.r_BcB_B = [0.0, 0.0, 0.0] + sc.hub.IHubPntBc_B = [[IXX, 0.0, 0.0], [0.0, IYY, 0.0], [0.0, 0.0, IZZ]] + + r0 = EARTH_RADIUS_M + ALTITUDE_M + v0 = math.sqrt(MU_EARTH / r0) + inc = math.radians(INCLINATION_DEG) + sc.hub.r_CN_NInit = [r0, 0.0, 0.0] + sc.hub.v_CN_NInit = [0.0, v0 * math.cos(inc), v0 * math.sin(inc)] + sc.hub.sigma_BNInit = [0.0, 0.0, 0.0] + sc.hub.omega_BN_BInit = [INITIAL_RATES[0], INITIAL_RATES[1], INITIAL_RATES[2]] + + earth = gravityEffector.GravBodyData() + earth.planetName = "earth_planet_data" + earth.mu = MU_EARTH + earth.isCentralBody = True + sc.gravField.gravBodies = spacecraft.GravBodyVector([earth]) + return sc + + +def configure_mtb_config_message(config: ADCSConfig): + """Create MTB layout message. + + Basilisk's MtbEffector reads GtMatrix_B as a row-major 3 x numMTB + matrix. For three body-aligned torque bars, the active portion must be + + [1, 0, 0, + 0, 1, 0, + 0, 0, 1] + + Do not stride by MAX_EFF_CNT here. MtbEffector only reads the first + 3*numMTB entries of this array. + """ + gt = [0.0] * (3 * MAX_EFF_CNT) + gt[0] = 1.0 + gt[4] = 1.0 + gt[8] = 1.0 + + max_dipoles = [0.0] * MAX_EFF_CNT + max_dipoles[0:3] = list(config.mtqDipoleLimit_Am2) + + payload = messaging.MTBArrayConfigMsgPayload() + payload.numMTB = 3 + payload.GtMatrix_B = gt + payload.maxMtbDipoles = max_dipoles + return messaging.MTBArrayConfigMsg().write(payload) + + +def run(): + sim = SimulationBaseClass.SimBaseClass() + proc = sim.CreateNewProcess("DynamicsProcess") + proc.addTask(sim.CreateNewTask("DynamicsTask", macros.sec2nano(CONTROL_DT_S))) + + sc = configure_spacecraft() + + mag = magneticFieldWMM.MagneticFieldWMM() + mag.ModelTag = "WMM2025" + mag.wmmDataFullPath = find_wmm2025_path() + mag.epochDateFractionalYear = EPOCH_FRACTIONAL_YEAR + mag.planetRadius = 6371.2e3 + mag.addSpacecraftToModel(sc.scStateOutMsg) + + nav = simpleNav.SimpleNav() + nav.ModelTag = "SimpleNav" + nav.scStateInMsg.subscribeTo(sc.scStateOutMsg) + + tam = magnetometer.Magnetometer() + tam.ModelTag = "Magnetometer" + tam.dcm_SB = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + tam.scaleFactor = 1.0 + tam.senBias = [0.0, 0.0, 0.0] + tam.senNoiseStd = [MAG_NOISE_STD_T, MAG_NOISE_STD_T, MAG_NOISE_STD_T] + tam.stateInMsg.subscribeTo(sc.scStateOutMsg) + tam.magInMsg.subscribeTo(mag.envOutMsgs[0]) + + adcs_cfg = ADCSConfig() + ctrl = PythonBdotMTQController(adcs_cfg) + ctrl.ModelTag = "PythonBdotMTQController" + ctrl.navAttInMsg.subscribeTo(nav.attOutMsg) + ctrl.tamSensorInMsg.subscribeTo(tam.tamDataOutMsg) + + mtb_cfg_msg = configure_mtb_config_message(adcs_cfg) + + # Primary fast-development actuator path for now: + # The controller computes the magnetic torque tau_B = m_B x B_B and sends it to + # Basilisk's native ExtForceTorque dynamic effector. This proves the Basilisk + # spacecraft dynamics/control loop without requiring a custom Basilisk C++ module. + # MtbEffector remains the target native MTB actuator, but is not used here because + # its output was zero in the first local test. + direct_torque = extForceTorque.ExtForceTorque() + direct_torque.ModelTag = "DirectMagneticTorqueFallback" + direct_torque.cmdTorqueInMsg.subscribeTo(ctrl.cmdTorqueOutMsg) + sc.addDynamicEffector(direct_torque) + + # Keep a native MtbEffector object available for later debugging, but do not attach + # it while the direct torque bridge is active; otherwise torque could be double-counted + # once MtbEffector wiring is fixed. + mtb = MtbEffector.MtbEffector() + mtb.ModelTag = "MtbEffector_debug_not_attached" + mtb.mtbCmdInMsg.subscribeTo(ctrl.mtbCmdOutMsg) + mtb.magInMsg.subscribeTo(mag.envOutMsgs[0]) + mtb.mtbParamsInMsg.subscribeTo(mtb_cfg_msg) + + # Higher priority runs earlier. Controller writes command before torque/dynamics updates. + sim.AddModelToTask("DynamicsTask", mag, ModelPriority=900) + sim.AddModelToTask("DynamicsTask", nav, ModelPriority=800) + sim.AddModelToTask("DynamicsTask", tam, ModelPriority=700) + sim.AddModelToTask("DynamicsTask", ctrl, ModelPriority=600) + sim.AddModelToTask("DynamicsTask", direct_torque, ModelPriority=500) + sim.AddModelToTask("DynamicsTask", sc, ModelPriority=100) + + rec_dt = macros.sec2nano(RECORD_DT_S) + sc_log = sc.scStateOutMsg.recorder(rec_dt) + mag_log = mag.envOutMsgs[0].recorder(rec_dt) + tam_log = tam.tamDataOutMsg.recorder(rec_dt) + cmd_log = ctrl.mtbCmdOutMsg.recorder(rec_dt) + direct_torque_log = direct_torque.logger(["torqueExternalPntB_B"]) + nav_log = nav.attOutMsg.recorder(rec_dt) + for recorder in [sc_log, mag_log, tam_log, cmd_log, direct_torque_log, nav_log]: + sim.AddModelToTask("DynamicsTask", recorder) + + r0 = EARTH_RADIUS_M + ALTITUDE_M + orbit_period_s = 2.0 * math.pi * math.sqrt(r0**3 / MU_EARTH) + print(f"Running detumble scenario for one orbit: {orbit_period_s:.2f} s") + + sim.InitializeSimulation() + sim.ConfigureStopTime(macros.sec2nano(orbit_period_s)) + sim.ExecuteSimulation() + + t = sc_log.times() * macros.NANO2SEC + r = np.asarray(sc_log.r_BN_N, dtype=float) + v = np.asarray(sc_log.v_BN_N, dtype=float) + sigma = np.asarray(sc_log.sigma_BN, dtype=float) + omega = np.asarray(sc_log.omega_BN_B, dtype=float) + omega_mag = np.linalg.norm(omega, axis=1) + euler = np.array([dcm_to_euler321(mrp_to_dcm(s)) for s in sigma]) + + B_N = np.asarray(mag_log.magField_N, dtype=float) + B_B = np.asarray(tam_log.tam_S, dtype=float) + dipole_all = np.asarray(cmd_log.mtbDipoleCmds, dtype=float) + dipole = dipole_all[:, :3] + # The direct_torque logger samples at the task rate, while the spacecraft + # recorders sample at RECORD_DT_S. Align the direct-torque log onto the + # canonical spacecraft time vector before building the output DataFrame. + applied_torque_raw = np.asarray(direct_torque_log.torqueExternalPntB_B, dtype=float) + applied_torque_times = np.asarray(direct_torque_log.times(), dtype=float) * macros.NANO2SEC + + if applied_torque_raw.ndim == 1: + applied_torque_raw = applied_torque_raw.reshape((-1, 3)) + + if len(applied_torque_raw) == 0: + applied_torque_B = np.zeros((len(t), 3)) + else: + n_torque = min(len(applied_torque_raw), len(applied_torque_times)) + torque_df = pd.DataFrame({ + "time_s": applied_torque_times[:n_torque], + "applied_torque_B_x_Nm": applied_torque_raw[:n_torque, 0], + "applied_torque_B_y_Nm": applied_torque_raw[:n_torque, 1], + "applied_torque_B_z_Nm": applied_torque_raw[:n_torque, 2], + }).sort_values("time_s") + + torque_sampled = pd.merge_asof( + pd.DataFrame({"time_s": t}), + torque_df, + on="time_s", + direction="nearest", + tolerance=max(CONTROL_DT_S, RECORD_DT_S), + ) + applied_torque_B = torque_sampled[[ + "applied_torque_B_x_Nm", + "applied_torque_B_y_Nm", + "applied_torque_B_z_Nm", + ]].fillna(0.0).to_numpy(dtype=float) + + diag = pd.DataFrame(ctrl.history) + diag_sampled = pd.merge_asof( + pd.DataFrame({"time_s": t}), + diag.sort_values("time_s") if not diag.empty else pd.DataFrame({"time_s": t}), + on="time_s", + direction="nearest", + tolerance=CONTROL_DT_S, + ) + + df = pd.DataFrame({ + "time_s": t, + "r_N_x_m": r[:, 0], "r_N_y_m": r[:, 1], "r_N_z_m": r[:, 2], + "v_N_x_m_s": v[:, 0], "v_N_y_m_s": v[:, 1], "v_N_z_m_s": v[:, 2], + "sigma_BN_1": sigma[:, 0], "sigma_BN_2": sigma[:, 1], "sigma_BN_3": sigma[:, 2], + "euler321_phi_rad": euler[:, 0], + "euler321_theta_rad": euler[:, 1], + "euler321_psi_rad": euler[:, 2], + "euler321_phi_deg": np.degrees(euler[:, 0]), + "euler321_theta_deg": np.degrees(euler[:, 1]), + "euler321_psi_deg": np.degrees(euler[:, 2]), + "omega_B_x_rad_s": omega[:, 0], "omega_B_y_rad_s": omega[:, 1], "omega_B_z_rad_s": omega[:, 2], + "omega_mag_rad_s": omega_mag, + "B_N_x_T": B_N[:, 0], "B_N_y_T": B_N[:, 1], "B_N_z_T": B_N[:, 2], + "B_N_mag_T": np.linalg.norm(B_N, axis=1), + "B_B_x_T": B_B[:, 0], "B_B_y_T": B_B[:, 1], "B_B_z_T": B_B[:, 2], + "B_B_mag_T": np.linalg.norm(B_B, axis=1), + "mcmd_x_Am2": dipole[:, 0], "mcmd_y_Am2": dipole[:, 1], "mcmd_z_Am2": dipole[:, 2], + "applied_torque_B_x_Nm": applied_torque_B[:, 0], + "applied_torque_B_y_Nm": applied_torque_B[:, 1], + "applied_torque_B_z_Nm": applied_torque_B[:, 2], + "applied_torque_B_mag_Nm": np.linalg.norm(applied_torque_B, axis=1), + # Legacy names retained so plotting/comparison scripts keep working. These are direct applied magnetic torque values. + "mtb_torque_B_x_Nm": applied_torque_B[:, 0], + "mtb_torque_B_y_Nm": applied_torque_B[:, 1], + "mtb_torque_B_z_Nm": applied_torque_B[:, 2], + "mtb_torque_B_mag_Nm": np.linalg.norm(applied_torque_B, axis=1), + }) + + # Use the controller diagnostic history as the authoritative source for + # command-side quantities. The Basilisk message recorders and dynamic + # effector logger can be sampled at slightly different phases. Mixing those + # sources caused false validation failures in the m x B consistency check. + # These columns are intended to represent the controller/update-time command + # state sampled onto the canonical 1 Hz output grid. + authoritative_diag_cols = [ + "B_B_x_T", "B_B_y_T", "B_B_z_T", + "mcmd_x_Am2", "mcmd_y_Am2", "mcmd_z_Am2", + "ix_A", "iy_A", "iz_A", + "pcoil_x_W", "pcoil_y_W", "pcoil_z_W", "pcoil_total_W", + "control_torque_B_x_Nm", "control_torque_B_y_Nm", "control_torque_B_z_Nm", "control_torque_B_mag_Nm", + "saturation_x", "saturation_y", "saturation_z", + "controller_valid", "using_cpp_core", + ] + for col in authoritative_diag_cols: + if col in diag_sampled.columns: + df[col] = diag_sampled[col] + + # In the direct-torque fallback, the controller torque command is the torque + # sent to Basilisk's ExtForceTorque effector. Record it consistently with the + # same sampled controller diagnostics so validation checks compare like with + # like: tau_B = m_B x B_B. + torque_cols = ["control_torque_B_x_Nm", "control_torque_B_y_Nm", "control_torque_B_z_Nm"] + if all(col in df.columns for col in torque_cols): + torque_cmd = df[torque_cols].to_numpy(dtype=float) + torque_cmd_mag = np.linalg.norm(torque_cmd, axis=1) + df["applied_torque_B_x_Nm"] = torque_cmd[:, 0] + df["applied_torque_B_y_Nm"] = torque_cmd[:, 1] + df["applied_torque_B_z_Nm"] = torque_cmd[:, 2] + df["applied_torque_B_mag_Nm"] = torque_cmd_mag + df["mtb_torque_B_x_Nm"] = torque_cmd[:, 0] + df["mtb_torque_B_y_Nm"] = torque_cmd[:, 1] + df["mtb_torque_B_z_Nm"] = torque_cmd[:, 2] + df["mtb_torque_B_mag_Nm"] = torque_cmd_mag + # Keep the printed summary aligned with the written CSV. + applied_torque_B = torque_cmd + + out_csv = OUT_DATA / "detumble_output.csv" + df.to_csv(out_csv, index=False) + print(f"Wrote {out_csv}") + print(f"Initial |omega| [rad/s]: {omega_mag[0]:.12g}") + print(f"Final |omega| [rad/s]: {omega_mag[-1]:.12g}") + print(f"Mean |applied magnetic torque| [N m]: {np.linalg.norm(applied_torque_B, axis=1).mean():.12g}") + print(f"Peak |applied magnetic torque| [N m]: {np.linalg.norm(applied_torque_B, axis=1).max():.12g}") + + from plot_results import plot_all + plot_all(df, OUT_PLOTS) + return df + + +if __name__ == "__main__": + run() diff --git a/basilisk_runner/scenario_huskysat2_minimal.py b/basilisk_runner/scenario_huskysat2_minimal.py new file mode 100644 index 0000000..d14eb98 --- /dev/null +++ b/basilisk_runner/scenario_huskysat2_minimal.py @@ -0,0 +1,152 @@ +"""Minimal HuskySat-style Basilisk spacecraft/orbit/attitude scenario. + +No custom C++ and no Basilisk ExternalModules are required. +""" + +from __future__ import annotations + +from pathlib import Path +import math +import sys + +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + +try: + from Basilisk.utilities import SimulationBaseClass, macros + from Basilisk.simulation import spacecraft, gravityEffector +except ImportError as exc: + raise SystemExit( + "Basilisk is not installed in this Python environment. Run: python -m pip install -r requirements.txt" + ) from exc + +HERE = Path(__file__).resolve().parent +OUT_DATA = HERE / "output_data" +OUT_PLOTS = HERE / "output_plots" +OUT_DATA.mkdir(parents=True, exist_ok=True) +OUT_PLOTS.mkdir(parents=True, exist_ok=True) + +MASS_KG = 2.6 +LX_M, LY_M, LZ_M = 0.10, 0.10, 0.20 +IXX = (MASS_KG / 12.0) * (LY_M**2 + LZ_M**2) +IYY = (MASS_KG / 12.0) * (LX_M**2 + LZ_M**2) +IZZ = (MASS_KG / 12.0) * (LX_M**2 + LY_M**2) +MU_EARTH = 3.986004418e14 +EARTH_RADIUS_M = 6.371e6 +ALTITUDE_M = 600e3 +INCLINATION_DEG = 56.0 +INITIAL_RATES = [0.8, -0.2, 0.3] +TASK_DT_S = 0.1 +RECORD_DT_S = 1.0 +RUN_TIME_S = 600.0 + + +def mrp_to_dcm(sigma): + s = np.asarray(sigma, dtype=float) + s2 = float(np.dot(s, s)) + sx = np.array([[0.0, -s[2], s[1]], [s[2], 0.0, -s[0]], [-s[1], s[0], 0.0]]) + return np.eye(3) + (8.0 * sx @ sx - 4.0 * (1.0 - s2) * sx) / (1.0 + s2) ** 2 + + +def dcm_to_euler321(C): + C = np.asarray(C, dtype=float) + phi = math.atan2(C[1, 2], C[2, 2]) + theta = -math.asin(float(np.clip(C[0, 2], -1.0, 1.0))) + psi = math.atan2(C[0, 1], C[0, 0]) + return np.array([phi, theta, psi]) + + +def configure_spacecraft(): + sc = spacecraft.Spacecraft() + sc.ModelTag = "HuskySat2_minimal" + sc.hub.mHub = MASS_KG + sc.hub.r_BcB_B = [0.0, 0.0, 0.0] + sc.hub.IHubPntBc_B = [[IXX, 0.0, 0.0], [0.0, IYY, 0.0], [0.0, 0.0, IZZ]] + + r0 = EARTH_RADIUS_M + ALTITUDE_M + v0 = math.sqrt(MU_EARTH / r0) + inc = math.radians(INCLINATION_DEG) + sc.hub.r_CN_NInit = [r0, 0.0, 0.0] + sc.hub.v_CN_NInit = [0.0, v0 * math.cos(inc), v0 * math.sin(inc)] + sc.hub.sigma_BNInit = [0.0, 0.0, 0.0] + sc.hub.omega_BN_BInit = [INITIAL_RATES[0], INITIAL_RATES[1], INITIAL_RATES[2]] + + earth = gravityEffector.GravBodyData() + earth.planetName = "earth_planet_data" + earth.mu = MU_EARTH + earth.isCentralBody = True + sc.gravField.gravBodies = spacecraft.GravBodyVector([earth]) + return sc + + +def run(run_time_s: float = RUN_TIME_S): + sim = SimulationBaseClass.SimBaseClass() + proc = sim.CreateNewProcess("DynamicsProcess") + proc.addTask(sim.CreateNewTask("DynamicsTask", macros.sec2nano(TASK_DT_S))) + + sc = configure_spacecraft() + sim.AddModelToTask("DynamicsTask", sc) + + rec = sc.scStateOutMsg.recorder(macros.sec2nano(RECORD_DT_S)) + sim.AddModelToTask("DynamicsTask", rec) + + sim.InitializeSimulation() + sim.ConfigureStopTime(macros.sec2nano(run_time_s)) + sim.ExecuteSimulation() + + t = rec.times() * macros.NANO2SEC + r = np.asarray(rec.r_BN_N, dtype=float) + v = np.asarray(rec.v_BN_N, dtype=float) + sigma = np.asarray(rec.sigma_BN, dtype=float) + omega = np.asarray(rec.omega_BN_B, dtype=float) + omega_mag = np.linalg.norm(omega, axis=1) + euler = np.array([dcm_to_euler321(mrp_to_dcm(s)) for s in sigma]) + + df = pd.DataFrame({ + "time_s": t, + "r_N_x_m": r[:, 0], "r_N_y_m": r[:, 1], "r_N_z_m": r[:, 2], + "v_N_x_m_s": v[:, 0], "v_N_y_m_s": v[:, 1], "v_N_z_m_s": v[:, 2], + "sigma_BN_1": sigma[:, 0], "sigma_BN_2": sigma[:, 1], "sigma_BN_3": sigma[:, 2], + "omega_B_x_rad_s": omega[:, 0], "omega_B_y_rad_s": omega[:, 1], "omega_B_z_rad_s": omega[:, 2], + "omega_mag_rad_s": omega_mag, + "euler321_phi_rad": euler[:, 0], "euler321_theta_rad": euler[:, 1], "euler321_psi_rad": euler[:, 2], + }) + out_csv = OUT_DATA / "minimal_output.csv" + df.to_csv(out_csv, index=False) + + plt.figure(figsize=(9, 4)) + plt.plot(t, omega[:, 0], label="p") + plt.plot(t, omega[:, 1], label="q") + plt.plot(t, omega[:, 2], label="r") + plt.plot(t, omega_mag, label="|omega|", linewidth=2) + plt.xlabel("Time [s]") + plt.ylabel("Rate [rad/s]") + plt.title("Minimal Basilisk Body Rates") + plt.grid(True) + plt.legend() + plt.tight_layout() + plt.savefig(OUT_PLOTS / "body_rates.png", dpi=180) + plt.close() + + plt.figure(figsize=(9, 4)) + plt.plot(t, euler[:, 0], label="phi") + plt.plot(t, euler[:, 1], label="theta") + plt.plot(t, euler[:, 2], label="psi") + plt.xlabel("Time [s]") + plt.ylabel("Angle [rad]") + plt.title("Minimal Basilisk Euler 321 Angles") + plt.grid(True) + plt.legend() + plt.tight_layout() + plt.savefig(OUT_PLOTS / "nav_euler.png", dpi=180) + plt.close() + + print(f"Wrote {out_csv}") + print(f"Initial |omega| [rad/s]: {omega_mag[0]:.12g}") + print(f"Final |omega| [rad/s]: {omega_mag[-1]:.12g}") + return df + + +if __name__ == "__main__": + run() diff --git a/basilisk_runner/scenario_huskysat2_pointing.py b/basilisk_runner/scenario_huskysat2_pointing.py new file mode 100644 index 0000000..4ee59f4 --- /dev/null +++ b/basilisk_runner/scenario_huskysat2_pointing.py @@ -0,0 +1,406 @@ +"""HuskySat-style Basilisk pointing proof-of-concept. + +This is intentionally a proof-of-concept, not the final magnetorquer-only +pointing controller. It uses Basilisk for the spacecraft/orbit/attitude plant +and applies a direct body torque through extForceTorque. The controller is a +small Python MRP PD law so we can validate pointing-error reduction before +adding magnetorquer-only torque allocation. + +Fast workflow properties: +- pip-installed Basilisk +- no Basilisk source rebuild +- no Basilisk.ExternalModules import +- no custom Basilisk C++ module +""" + +from __future__ import annotations + +from pathlib import Path +import json +import math + +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + +try: + from Basilisk.utilities import SimulationBaseClass, macros + from Basilisk.simulation import simpleNav, extForceTorque + from Basilisk.architecture import sysModel, messaging, bskLogging +except ImportError as exc: + raise SystemExit( + "Basilisk is not installed in this Python environment. Run: python -m pip install -r requirements.txt" + ) from exc + +from scenario_huskysat2_detumble import ( + configure_spacecraft, + mrp_to_dcm, + dcm_to_euler321, + IXX, + IYY, + IZZ, +) + +HERE = Path(__file__).resolve().parent +OUT_DATA = HERE / "output_data" +OUT_PLOTS = HERE / "output_plots" +OUT_DATA.mkdir(parents=True, exist_ok=True) +OUT_PLOTS.mkdir(parents=True, exist_ok=True) + +CONTROL_DT_S = 0.1 +RECORD_DT_S = 1.0 +SIM_DURATION_S = 1800.0 + +# A nonzero initial attitude error and modest initial body rates make this a +# real pointing test instead of merely holding an already aligned attitude. +POINTING_INITIAL_SIGMA_BN = [0.22, -0.12, 0.16] +POINTING_INITIAL_RATES_RAD_S = [0.03, -0.02, 0.015] + +# Direct-torque MRP PD gains. These are intentionally conservative for the +# small CubeSat inertia used in the detumble baseline. +K_MRP_NM = 8.0e-5 +P_RATE_NM_PER_RAD_S = 1.8e-3 +MAX_TORQUE_NM = 2.0e-4 + + +def mrp_shadow_if_needed(sigma): + """Return the MRP set with norm <= 1.""" + s = np.asarray(sigma, dtype=float) + s2 = float(np.dot(s, s)) + if s2 > 1.0: + return -s / s2 + return s + + +def mrp_angle_deg(sigma): + """Return equivalent principal rotation angle for an MRP vector.""" + s = mrp_shadow_if_needed(sigma) + angle_rad = 4.0 * math.atan(float(np.linalg.norm(s))) + if angle_rad > math.pi: + angle_rad = 2.0 * math.pi - angle_rad + return math.degrees(angle_rad) + + +class PythonMRPDirectTorqueController(sysModel.SysModel): + """Direct body-torque MRP PD controller for pointing bring-up. + + Reference attitude: inertial identity frame, sigma_RN = 0. + Error attitude: because the reference is identity and non-rotating, the + body-to-reference error is represented by sigma_BN. + + Output: CmdTorqueBodyMsgPayload.torqueRequestBody [N m]. + """ + + def __init__(self, *args): + super().__init__(*args) + self.navAttInMsg = messaging.NavAttMsgReader() + self.cmdTorqueOutMsg = messaging.CmdTorqueBodyMsg() + self.history = [] + + def Reset(self, CurrentSimNanos): + if not self.navAttInMsg.isLinked(): + self.bskLogger.bskLog(bskLogging.BSK_ERROR, "PythonMRPDirectTorqueController.navAttInMsg is not linked.") + payload = self.cmdTorqueOutMsg.zeroMsgPayload + payload.torqueRequestBody = [0.0, 0.0, 0.0] + self.cmdTorqueOutMsg.write(payload, CurrentSimNanos, self.moduleID) + self.history.clear() + + def UpdateState(self, CurrentSimNanos): + nav = self.navAttInMsg() + time_s = CurrentSimNanos * 1.0e-9 + + sigma_BR = mrp_shadow_if_needed(np.asarray(nav.sigma_BN, dtype=float)[:3]) + omega_BR_B = np.asarray(nav.omega_BN_B, dtype=float)[:3] + + raw_torque = -K_MRP_NM * sigma_BR - P_RATE_NM_PER_RAD_S * omega_BR_B + torque_mag = float(np.linalg.norm(raw_torque)) + if torque_mag > MAX_TORQUE_NM: + torque = raw_torque * (MAX_TORQUE_NM / torque_mag) + saturated = True + else: + torque = raw_torque + saturated = False + + payload = self.cmdTorqueOutMsg.zeroMsgPayload + payload.torqueRequestBody = torque.tolist() + self.cmdTorqueOutMsg.write(payload, CurrentSimNanos, self.moduleID) + + self.history.append({ + "time_s": time_s, + "sigma_BR_1": float(sigma_BR[0]), + "sigma_BR_2": float(sigma_BR[1]), + "sigma_BR_3": float(sigma_BR[2]), + "sigma_BR_mag": float(np.linalg.norm(sigma_BR)), + "pointing_error_deg": mrp_angle_deg(sigma_BR), + "omega_B_x_rad_s": float(omega_BR_B[0]), + "omega_B_y_rad_s": float(omega_BR_B[1]), + "omega_B_z_rad_s": float(omega_BR_B[2]), + "omega_mag_rad_s": float(np.linalg.norm(omega_BR_B)), + "torque_B_x_Nm": float(torque[0]), + "torque_B_y_Nm": float(torque[1]), + "torque_B_z_Nm": float(torque[2]), + "torque_B_mag_Nm": float(np.linalg.norm(torque)), + "raw_torque_B_mag_Nm": torque_mag, + "saturated": saturated, + }) + + +def configure_pointing_spacecraft(): + sc = configure_spacecraft() + sc.ModelTag = "HuskySat2_pointing_poc" + sc.hub.sigma_BNInit = POINTING_INITIAL_SIGMA_BN + sc.hub.omega_BN_BInit = POINTING_INITIAL_RATES_RAD_S + return sc + + +def plot_pointing(df: pd.DataFrame): + fig, ax = plt.subplots(figsize=(9, 4.8)) + ax.plot(df["time_s"], df["pointing_error_deg"], label="Pointing error") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Pointing error [deg]") + ax.grid(True) + ax.legend() + fig.tight_layout() + path = OUT_PLOTS / "pointing_error.png" + fig.savefig(path, dpi=180) + plt.close(fig) + print(f"Saved {path}") + + fig, ax = plt.subplots(figsize=(9, 4.8)) + ax.plot(df["time_s"], df["omega_B_x_rad_s"], label="omega_x") + ax.plot(df["time_s"], df["omega_B_y_rad_s"], label="omega_y") + ax.plot(df["time_s"], df["omega_B_z_rad_s"], label="omega_z") + ax.plot(df["time_s"], df["omega_mag_rad_s"], label="|omega|", linewidth=2) + ax.set_xlabel("Time [s]") + ax.set_ylabel("Body rate [rad/s]") + ax.grid(True) + ax.legend() + fig.tight_layout() + path = OUT_PLOTS / "pointing_body_rates.png" + fig.savefig(path, dpi=180) + plt.close(fig) + print(f"Saved {path}") + + fig, ax = plt.subplots(figsize=(9, 4.8)) + ax.plot(df["time_s"], df["torque_B_x_Nm"], label="tau_x") + ax.plot(df["time_s"], df["torque_B_y_Nm"], label="tau_y") + ax.plot(df["time_s"], df["torque_B_z_Nm"], label="tau_z") + ax.plot(df["time_s"], df["torque_B_mag_Nm"], label="|tau|", linewidth=2) + ax.set_xlabel("Time [s]") + ax.set_ylabel("Direct body torque [N m]") + ax.grid(True) + ax.legend() + fig.tight_layout() + path = OUT_PLOTS / "pointing_torque.png" + fig.savefig(path, dpi=180) + plt.close(fig) + print(f"Saved {path}") + + fig, ax = plt.subplots(figsize=(9, 4.8)) + ax.plot(df["time_s"], df["euler321_phi_deg"], label="phi") + ax.plot(df["time_s"], df["euler321_theta_deg"], label="theta") + ax.plot(df["time_s"], df["euler321_psi_deg"], label="psi") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Euler 3-2-1 angle [deg]") + ax.grid(True) + ax.legend() + fig.tight_layout() + path = OUT_PLOTS / "pointing_euler.png" + fig.savefig(path, dpi=180) + plt.close(fig) + print(f"Saved {path}") + + fig, ax = plt.subplots(figsize=(9, 4.8)) + ax.plot(df["time_s"], df["pointing_error_deg"], label="Pointing error [deg]") + ax2 = ax.twinx() + ax2.plot(df["time_s"], df["omega_mag_rad_s"], label="|omega| [rad/s]", linestyle="--") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Pointing error [deg]") + ax2.set_ylabel("Angular speed [rad/s]") + ax.grid(True) + lines, labels = ax.get_legend_handles_labels() + lines2, labels2 = ax2.get_legend_handles_labels() + ax.legend(lines + lines2, labels + labels2, loc="best") + fig.tight_layout() + path = OUT_PLOTS / "pointing_summary.png" + fig.savefig(path, dpi=180) + plt.close(fig) + print(f"Saved {path}") + + +def _check(name: str, passed: bool, value=None, limit=None): + return name, {"passed": bool(passed), "value": value, "limit": limit} + + +def write_pointing_metrics(df: pd.DataFrame) -> dict: + """Write pass/fail validation metrics for the pointing proof-of-concept. + + These are engineering sanity checks for the direct-torque pointing bring-up. + They are not a claim that the final magnetorquer-limited controller is done. + """ + numeric = df.select_dtypes(include=[np.number]) + finite_numeric = bool(np.isfinite(numeric.to_numpy()).all()) + + initial_error = float(df["pointing_error_deg"].iloc[0]) + final_error = float(df["pointing_error_deg"].iloc[-1]) + initial_rate = float(df["omega_mag_rad_s"].iloc[0]) + final_rate = float(df["omega_mag_rad_s"].iloc[-1]) + peak_torque = float(df["torque_B_mag_Nm"].max()) + saturation_fraction = float(df["saturated"].astype(bool).mean()) if "saturated" in df else float("nan") + + omega = df[["omega_B_x_rad_s", "omega_B_y_rad_s", "omega_B_z_rad_s"]].to_numpy(dtype=float) + inertia_diag = np.array([IXX, IYY, IZZ], dtype=float) + rotational_energy = 0.5 * np.sum((omega ** 2) * inertia_diag[None, :], axis=1) + initial_energy = float(rotational_energy[0]) + final_energy = float(rotational_energy[-1]) + + checks = dict([ + _check("numeric_finite", finite_numeric, None, None), + _check("final_pointing_error_less_than_initial", final_error < initial_error, final_error, f"< {initial_error}"), + _check("final_pointing_error_below_1_deg", final_error < 1.0, final_error, "< 1 deg"), + _check("final_omega_less_than_initial", final_rate < initial_rate, final_rate, f"< {initial_rate}"), + _check("final_omega_below_1e_minus_4_rad_s", final_rate < 1.0e-4, final_rate, "< 1e-4 rad/s"), + _check("final_rotational_energy_less_than_initial", final_energy < initial_energy, final_energy, f"< {initial_energy}"), + _check("peak_torque_within_limit", peak_torque <= MAX_TORQUE_NM * 1.000001, peak_torque, f"<= {MAX_TORQUE_NM} N m"), + _check("saturation_fraction_small", saturation_fraction <= 0.05, saturation_fraction, "<= 0.05"), + ]) + + passed = bool(all(item["passed"] for item in checks.values())) + metrics = { + "pointing_file": str(OUT_DATA / "pointing_output.csv"), + "scenario": "direct_torque_pointing_proof_of_concept", + "duration_s": float(df["time_s"].iloc[-1] - df["time_s"].iloc[0]), + "initial_pointing_error_deg": initial_error, + "final_pointing_error_deg": final_error, + "pointing_error_reduction_deg": initial_error - final_error, + "initial_omega_mag_rad_s": initial_rate, + "final_omega_mag_rad_s": final_rate, + "initial_rotational_energy_J": initial_energy, + "final_rotational_energy_J": final_energy, + "peak_direct_torque_Nm": peak_torque, + "torque_saturation_fraction": saturation_fraction, + "validation": { + "passed": passed, + "checks": checks, + }, + "notes": [ + "This validates the direct-torque pointing proof-of-concept only.", + "It does not validate final magnetorquer-only pointing or native MtbEffector integration.", + ], + } + + out_json = OUT_DATA / "pointing_metrics.json" + out_json.write_text(json.dumps(metrics, indent=2), encoding="utf-8") + print(f"Wrote {out_json}") + print(f"Pointing validation passed?: {passed}") + if not passed: + print("Failed pointing checks:") + for name, item in checks.items(): + if not item["passed"]: + print(f" - {name}") + return metrics + + +def run(duration_s: float = SIM_DURATION_S): + sim = SimulationBaseClass.SimBaseClass() + proc = sim.CreateNewProcess("PointingProcess") + proc.addTask(sim.CreateNewTask("PointingTask", macros.sec2nano(CONTROL_DT_S))) + + sc = configure_pointing_spacecraft() + + nav = simpleNav.SimpleNav() + nav.ModelTag = "SimpleNav_pointing" + nav.scStateInMsg.subscribeTo(sc.scStateOutMsg) + + ctrl = PythonMRPDirectTorqueController() + ctrl.ModelTag = "PythonMRPDirectTorqueController" + ctrl.navAttInMsg.subscribeTo(nav.attOutMsg) + + direct_torque = extForceTorque.ExtForceTorque() + direct_torque.ModelTag = "DirectPointingTorque" + direct_torque.cmdTorqueInMsg.subscribeTo(ctrl.cmdTorqueOutMsg) + sc.addDynamicEffector(direct_torque) + + sim.AddModelToTask("PointingTask", nav, ModelPriority=800) + sim.AddModelToTask("PointingTask", ctrl, ModelPriority=700) + sim.AddModelToTask("PointingTask", direct_torque, ModelPriority=600) + sim.AddModelToTask("PointingTask", sc, ModelPriority=100) + + rec_dt = macros.sec2nano(RECORD_DT_S) + sc_log = sc.scStateOutMsg.recorder(rec_dt) + nav_log = nav.attOutMsg.recorder(rec_dt) + sim.AddModelToTask("PointingTask", sc_log) + sim.AddModelToTask("PointingTask", nav_log) + + print(f"Running pointing proof-of-concept for {duration_s:.1f} s") + sim.InitializeSimulation() + sim.ConfigureStopTime(macros.sec2nano(duration_s)) + sim.ExecuteSimulation() + + t = sc_log.times() * macros.NANO2SEC + r = np.asarray(sc_log.r_BN_N, dtype=float) + v = np.asarray(sc_log.v_BN_N, dtype=float) + sigma = np.asarray(sc_log.sigma_BN, dtype=float) + omega = np.asarray(sc_log.omega_BN_B, dtype=float) + euler = np.array([dcm_to_euler321(mrp_to_dcm(s)) for s in sigma]) + + diag = pd.DataFrame(ctrl.history) + diag_sampled = pd.merge_asof( + pd.DataFrame({"time_s": t}), + diag.sort_values("time_s") if not diag.empty else pd.DataFrame({"time_s": t}), + on="time_s", + direction="nearest", + tolerance=CONTROL_DT_S, + ) + + df = pd.DataFrame({ + "time_s": t, + "r_N_x_m": r[:, 0], "r_N_y_m": r[:, 1], "r_N_z_m": r[:, 2], + "v_N_x_m_s": v[:, 0], "v_N_y_m_s": v[:, 1], "v_N_z_m_s": v[:, 2], + "sigma_BN_1": sigma[:, 0], "sigma_BN_2": sigma[:, 1], "sigma_BN_3": sigma[:, 2], + "euler321_phi_rad": euler[:, 0], + "euler321_theta_rad": euler[:, 1], + "euler321_psi_rad": euler[:, 2], + "euler321_phi_deg": np.degrees(euler[:, 0]), + "euler321_theta_deg": np.degrees(euler[:, 1]), + "euler321_psi_deg": np.degrees(euler[:, 2]), + "omega_B_x_rad_s": omega[:, 0], + "omega_B_y_rad_s": omega[:, 1], + "omega_B_z_rad_s": omega[:, 2], + "omega_mag_rad_s": np.linalg.norm(omega, axis=1), + "rotational_energy_J": 0.5 * (IXX * omega[:, 0]**2 + IYY * omega[:, 1]**2 + IZZ * omega[:, 2]**2), + }) + + for col in [ + "sigma_BR_1", "sigma_BR_2", "sigma_BR_3", "sigma_BR_mag", "pointing_error_deg", + "torque_B_x_Nm", "torque_B_y_Nm", "torque_B_z_Nm", "torque_B_mag_Nm", + "raw_torque_B_mag_Nm", "saturated", + ]: + if col in diag_sampled.columns: + df[col] = diag_sampled[col] + + out_csv = OUT_DATA / "pointing_output.csv" + df.to_csv(out_csv, index=False) + + initial_error = float(df["pointing_error_deg"].iloc[0]) + final_error = float(df["pointing_error_deg"].iloc[-1]) + initial_rate = float(df["omega_mag_rad_s"].iloc[0]) + final_rate = float(df["omega_mag_rad_s"].iloc[-1]) + max_torque = float(df["torque_B_mag_Nm"].max()) + saturated_fraction = float(df["saturated"].astype(bool).mean()) if "saturated" in df else float("nan") + + print(f"Wrote {out_csv}") + print(f"Initial pointing error [deg]: {initial_error:.12g}") + print(f"Final pointing error [deg]: {final_error:.12g}") + print(f"Initial |omega| [rad/s]: {initial_rate:.12g}") + print(f"Final |omega| [rad/s]: {final_rate:.12g}") + print(f"Peak direct torque [N m]: {max_torque:.12g}") + print(f"Torque saturation fraction: {saturated_fraction:.6g}") + + plot_pointing(df) + write_pointing_metrics(df) + return df + + +if __name__ == "__main__": + run() diff --git a/cpp_adcs_core/CMakeLists.txt b/cpp_adcs_core/CMakeLists.txt new file mode 100644 index 0000000..5eb7270 --- /dev/null +++ b/cpp_adcs_core/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.16) +project(cpp_adcs_core LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +add_library(adcs_core_lib STATIC + src/adcs_core.cpp +) +target_include_directories(adcs_core_lib PUBLIC include) + +add_executable(adcs_core_smoke + tests/test_adcs_core.cpp +) +target_link_libraries(adcs_core_smoke PRIVATE adcs_core_lib) + +option(ADCS_BUILD_PYBIND "Build optional pybind11 Python module" ON) +if(ADCS_BUILD_PYBIND) + find_package(pybind11 CONFIG QUIET) + if(pybind11_FOUND) + pybind11_add_module(adcs_core bindings/pybind11_module.cpp) + target_link_libraries(adcs_core PRIVATE adcs_core_lib) + target_include_directories(adcs_core PRIVATE include) + else() + message(WARNING "pybind11 was not found. Building C++ library and smoke test only. To build Python bindings, install pybind11 and pass -Dpybind11_DIR=$(python -m pybind11 --cmakedir).") + endif() +endif() diff --git a/cpp_adcs_core/bindings/pybind11_module.cpp b/cpp_adcs_core/bindings/pybind11_module.cpp new file mode 100644 index 0000000..df0406c --- /dev/null +++ b/cpp_adcs_core/bindings/pybind11_module.cpp @@ -0,0 +1,51 @@ +#include "adcs/adcs_core.hpp" + +#include +#include + +namespace py = pybind11; + +namespace { + +adcs::Vec3 as_vec3(const std::vector& v, const char* name) { + if (v.size() != 3) { + throw std::runtime_error(std::string(name) + " must have length 3"); + } + return adcs::Vec3{v[0], v[1], v[2]}; +} + +py::dict step_py(double time_s, + const std::vector& omega_B_rad_s, + const std::vector& mag_B_T, + py::dict config_dict = py::dict()) { + adcs::ControllerConfig cfg{}; + if (config_dict.contains("dipoleCommandGain")) cfg.dipoleCommandGain = config_dict["dipoleCommandGain"].cast(); + if (config_dict.contains("mtqDipoleGain_Am2_A")) cfg.mtqDipoleGain_Am2_A = as_vec3(config_dict["mtqDipoleGain_Am2_A"].cast>(), "mtqDipoleGain_Am2_A"); + if (config_dict.contains("mtqResistance_Ohm")) cfg.mtqResistance_Ohm = as_vec3(config_dict["mtqResistance_Ohm"].cast>(), "mtqResistance_Ohm"); + if (config_dict.contains("mtqCurrentLimit_A")) cfg.mtqCurrentLimit_A = as_vec3(config_dict["mtqCurrentLimit_A"].cast>(), "mtqCurrentLimit_A"); + if (config_dict.contains("mtqDipoleLimit_Am2")) cfg.mtqDipoleLimit_Am2 = as_vec3(config_dict["mtqDipoleLimit_Am2"].cast>(), "mtqDipoleLimit_Am2"); + if (config_dict.contains("minMagField_T")) cfg.minMagField_T = config_dict["minMagField_T"].cast(); + + adcs::StepInput input{}; + input.time_s = time_s; + input.omega_B_rad_s = as_vec3(omega_B_rad_s, "omega_B_rad_s"); + input.mag_B_T = as_vec3(mag_B_T, "mag_B_T"); + + const adcs::StepOutput out = adcs::step(input, cfg); + py::dict d; + d["commanded_magnetic_dipole_B_Am2"] = out.commandedDipole_B_Am2; + d["commanded_coil_current_A"] = out.commandedCurrent_A; + d["commanded_control_torque_B_Nm"] = out.commandedTorque_B_Nm; + d["coil_power_W"] = out.coilPower_W; + d["coil_power_total_W"] = out.coilPowerTotal_W; + d["saturation_flags"] = out.saturation; + d["valid"] = out.valid; + return d; +} + +} // namespace + +PYBIND11_MODULE(adcs_core, m) { + m.doc() = "Small standalone ADCS controller core for Basilisk adapter use."; + m.def("step", &step_py, py::arg("time_s"), py::arg("omega_B_rad_s"), py::arg("mag_B_T"), py::arg("config") = py::dict()); +} diff --git a/cpp_adcs_core/include/adcs/adcs_core.hpp b/cpp_adcs_core/include/adcs/adcs_core.hpp new file mode 100644 index 0000000..f936a1c --- /dev/null +++ b/cpp_adcs_core/include/adcs/adcs_core.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include +#include + +namespace adcs { + +using Vec3 = std::array; + +struct ControllerConfig { + double dipoleCommandGain = 67200.0; + Vec3 mtqDipoleGain_Am2_A{2.3, 2.3, 0.85 / 0.6301260378126048}; + Vec3 mtqResistance_Ohm{51.0, 51.0, 4.4}; + Vec3 mtqCurrentLimit_A{5.0 / 51.0, 5.0 / 51.0, 0.6301260378126048}; + Vec3 mtqDipoleLimit_Am2{0.2, 0.2, 0.85}; + double minMagField_T = 1.0e-12; +}; + +struct StepInput { + double time_s = 0.0; + Vec3 omega_B_rad_s{0.0, 0.0, 0.0}; + Vec3 mag_B_T{0.0, 0.0, 0.0}; +}; + +struct StepOutput { + Vec3 commandedDipole_B_Am2{0.0, 0.0, 0.0}; + Vec3 commandedCurrent_A{0.0, 0.0, 0.0}; + Vec3 commandedTorque_B_Nm{0.0, 0.0, 0.0}; + Vec3 coilPower_W{0.0, 0.0, 0.0}; + double coilPowerTotal_W = 0.0; + std::array saturation{false, false, false}; + bool valid = true; +}; + +StepOutput step(const StepInput& input, const ControllerConfig& config = ControllerConfig{}); + +} // namespace adcs diff --git a/cpp_adcs_core/src/adcs_core.cpp b/cpp_adcs_core/src/adcs_core.cpp new file mode 100644 index 0000000..5789ed4 --- /dev/null +++ b/cpp_adcs_core/src/adcs_core.cpp @@ -0,0 +1,82 @@ +#include "adcs/adcs_core.hpp" + +#include +#include + +namespace adcs { +namespace { + +Vec3 cross(const Vec3& a, const Vec3& b) { + return Vec3{ + a[1] * b[2] - a[2] * b[1], + a[2] * b[0] - a[0] * b[2], + a[0] * b[1] - a[1] * b[0], + }; +} + +double norm(const Vec3& v) { + return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); +} + +bool finite_vec(const Vec3& v) { + return std::isfinite(v[0]) && std::isfinite(v[1]) && std::isfinite(v[2]); +} + +double clamp(double value, double lo, double hi) { + return std::max(lo, std::min(value, hi)); +} + +} // namespace + +StepOutput step(const StepInput& input, const ControllerConfig& config) { + StepOutput out{}; + + if (!std::isfinite(input.time_s) || !finite_vec(input.omega_B_rad_s) || !finite_vec(input.mag_B_T)) { + out.valid = false; + return out; + } + + const double bmag = norm(input.mag_B_T); + if (!std::isfinite(bmag) || bmag < config.minMagField_T) { + out.valid = false; + return out; + } + + const Vec3 omega_cross_b = cross(input.omega_B_rad_s, input.mag_B_T); + Vec3 desiredDipole{ + config.dipoleCommandGain * omega_cross_b[0], + config.dipoleCommandGain * omega_cross_b[1], + config.dipoleCommandGain * omega_cross_b[2], + }; + + for (int i = 0; i < 3; ++i) { + const double gain = config.mtqDipoleGain_Am2_A[i]; + const double currentLimit = config.mtqCurrentLimit_A[i]; + const double dipoleLimit = config.mtqDipoleLimit_Am2[i]; + const double resistance = config.mtqResistance_Ohm[i]; + + if (gain <= 0.0 || currentLimit <= 0.0 || dipoleLimit <= 0.0 || resistance < 0.0) { + out.valid = false; + out.commandedCurrent_A[i] = 0.0; + out.commandedDipole_B_Am2[i] = 0.0; + out.coilPower_W[i] = 0.0; + out.saturation[i] = true; + continue; + } + + const double allowedCurrent = std::min(currentLimit, dipoleLimit / gain); + const double rawCurrent = desiredDipole[i] / gain; + const double cmdCurrent = clamp(rawCurrent, -allowedCurrent, allowedCurrent); + + out.commandedCurrent_A[i] = cmdCurrent; + out.commandedDipole_B_Am2[i] = cmdCurrent * gain; + out.coilPower_W[i] = resistance * cmdCurrent * cmdCurrent; + out.coilPowerTotal_W += out.coilPower_W[i]; + out.saturation[i] = std::abs(rawCurrent - cmdCurrent) > 1.0e-15; + } + + out.commandedTorque_B_Nm = cross(out.commandedDipole_B_Am2, input.mag_B_T); + return out; +} + +} // namespace adcs diff --git a/cpp_adcs_core/tests/test_adcs_core.cpp b/cpp_adcs_core/tests/test_adcs_core.cpp new file mode 100644 index 0000000..4f4ab07 --- /dev/null +++ b/cpp_adcs_core/tests/test_adcs_core.cpp @@ -0,0 +1,33 @@ +#include "adcs/adcs_core.hpp" + +#include +#include + +int main() { + adcs::StepInput input{}; + input.time_s = 0.0; + input.omega_B_rad_s = {0.8, -0.2, 0.3}; + input.mag_B_T = {1.0e-5, -1.6e-6, 2.06e-5}; + + const adcs::StepOutput out = adcs::step(input); + + if (!out.valid) { + std::cerr << "Expected valid controller output\n"; + return 1; + } + if (!std::isfinite(out.coilPowerTotal_W)) { + std::cerr << "Power is not finite\n"; + return 2; + } + + std::cout << "m_cmd = [" + << out.commandedDipole_B_Am2[0] << ", " + << out.commandedDipole_B_Am2[1] << ", " + << out.commandedDipole_B_Am2[2] << "] A m^2\n"; + std::cout << "i_cmd = [" + << out.commandedCurrent_A[0] << ", " + << out.commandedCurrent_A[1] << ", " + << out.commandedCurrent_A[2] << "] A\n"; + std::cout << "P_total = " << out.coilPowerTotal_W << " W\n"; + return 0; +} diff --git a/docs/basilisk_capability_audit.md b/docs/basilisk_capability_audit.md new file mode 100644 index 0000000..4e5fd1c --- /dev/null +++ b/docs/basilisk_capability_audit.md @@ -0,0 +1,69 @@ +# Basilisk capability audit + +This audit classifies each standalone subsystem before migration. The recommendation prioritizes Basilisk-native modules where practical and keeps C++ only for project-specific ADCS logic. + +## Classification key + +1. **Replace with Basilisk native module** +2. **Keep as custom C++ ADCS logic and expose through Python** +3. **Temporarily reimplement in Python for fast testing** +4. **Keep only as reference/validation tool** +5. **Unknown; needs investigation** + +## Subsystem table + +| Subsystem | Current standalone implementation | Basilisk capability | Recommendation | Notes/TODOs | +|---|---|---|---|---| +| Orbit dynamics | `src/satellite.cpp` manually integrates position/velocity with RK4 and central gravity plus disturbances. | `spacecraft.Spacecraft` provides 6-DOF rigid-body translational and rotational dynamics; gravity bodies can be attached. | **1** | Use Basilisk as the truth plant. Keep standalone only for validation. | +| Attitude dynamics | Quaternion propagation and Euler rigid-body equations in `src/satellite.cpp`. | `spacecraft.Spacecraft` propagates attitude with MRP state and body rates, including dynamic effectors. | **1** | Interface must convert standalone quaternion expectations to Basilisk MRPs where needed. | +| Attitude state representation | Scalar-first quaternion in standalone CSV. | Basilisk uses MRP `sigma_BN` and `omega_BN_B` in standard state messages. | **1 / 3** | Use MRP natively in Basilisk; convert to Euler/quaternion only for plotting or comparison. | +| WMM / magnetic field | Embedded WMM2025 coefficients in `src/wmm.cpp`; reference COF files also present. | `magneticFieldWMM.MagneticFieldWMM` computes Earth WMM field at spacecraft location. | **1**, with validation | Compare magnitude/history and frame conventions against standalone WMM. TODO: verify N-frame sign and epoch conventions. | +| Magnetic field messages | Standalone stores body, measured, and nav field vectors in `SimContext` and CSV. | Basilisk WMM publishes `MagneticFieldMsgPayload`; modules can subscribe and record. | **1** | Log both inertial magnetic field and magnetometer body-frame field. | +| Magnetometer | `src/sensor.cpp` adds fixed bias plus white noise. | `magnetometer.Magnetometer` models TAM measurements in sensor frame. | **1** | Use zero noise for first equivalence pass; add bias/noise after frame validation. | +| Magnetorquer / MTB effector | Current -> dipole in `src/satellite.cpp`; torque `m x B` applied manually. | `MtbEffector.MtbEffector` converts commanded MTB dipoles and WMM magnetic field into body torque. | **1** | Use Basilisk effector for torque application; keep current/dipole/power conversion as custom diagnostics. | +| Detumble controller | Placeholder rate-cross-field controller in `src/control.cpp`. | Basilisk has general attitude-control modules, but no project-specific HuskySat detumble law is guaranteed. Python modules are supported for prototyping. | **2 / 3** | First use Python Basilisk module for fast testing. Optionally call small pybind C++ core. Do not use Basilisk ExternalModules as first path. | +| Pointing control | Not clearly implemented as final controller in uploaded standalone; scaffold only. | `mrpFeedback` provides general MRP feedback control for reference tracking and can work with reaction wheels. | **1 / 3 / 5** | For magnetorquer-only pointing, custom logic may be needed because magnetic actuation is underactuated. Start after detumble is stable. | +| Reaction wheels | Not used in standalone. | Basilisk has reaction-wheel state effectors and MRP feedback support. | **5 later** | Useful later if HuskySat architecture includes RWs or for comparison. Not first migration path. | +| SRP | First-pass SRP force and optional zero-offset torque in `src/disturbance.cpp`. | Basilisk has radiation-pressure dynamic effectors, including cannonball/faceted style models depending on version. | **1 / 5** | Add only after minimal + magnetic detumble work. Need actual geometry/area/reflectivity. | +| Drag | Atmosphere-relative drag in `src/disturbance.cpp`; simple density table/model. | Basilisk has drag-related dynamic effectors and atmosphere/environment modules depending on selected fidelity. | **1 / 5** | Add after core ADCS validation. Need chosen density model and geometry. | +| Disturbance torque | Gravity-gradient, residual magnetic dipole, aero/SRP offset torques. | Basilisk provides native effectors for many disturbances; `extForceTorque` can apply custom disturbance/control torque if needed. | **1 / 3 / 5** | Gravity-gradient/residual magnetic torque need explicit Basilisk mapping or temporary adapter. Aero/SRP offsets are TODO until geometry known. | +| Coil power | `I^2 R` in `src/satellite.cpp`. | Basilisk has power modules, but coil electronics/power model is project-specific. | **2 / 3** | Keep in ADCS core or Python diagnostics; not part of truth plant. | +| Actuator saturation | Current and dipole saturation in standalone logic. | MTB config has dipole limits; custom current limits remain controller-side. | **2 / 3** | Keep current/dipole conversions and saturation in ADCS core/adapter. | +| Sensor/nav estimator | `src/navigation.cpp` lightweight gyro propagation + magnetic correction. | Basilisk `simpleNav` can produce truth-like navigation output; dedicated filters exist for some cases. | **1 / 3 / 5** | Use `simpleNav` for early plant/controller debug. Revisit estimator fidelity later. | +| Output/logging | Manual CSV in `src/main.cpp`. | Basilisk messages support `.recorder()` logging. | **1** | Save canonical CSV from message logs for comparison. | +| Plotting | `plot_all.py` reads standalone CSV. | Python plotting remains appropriate. | **3 / 4** | New plotter reads canonical Basilisk CSV. Old plotter remains reference. | +| Validation | Known-good standalone metrics and plots. | Basilisk does not replace validation. | **4** | Preserve standalone as baseline and compare physical metrics. | +| Monte Carlo / tests | Not visible in standalone. | Basilisk supports repeatable Monte Carlo and pytest-based testing. | **1 later** | Add after nominal scenario is stable. | + +## Main recommendation + +Use Basilisk natively for the simulation environment: + +- orbit and attitude propagation +- gravity +- WMM magnetic field +- magnetic-field messages +- magnetometer sensor +- magnetorquer torque application +- message logging +- later: SRP, drag, Monte Carlo, reaction wheels if needed + +Keep project-specific ADCS logic outside Basilisk: + +- detumble law +- pointing/mode logic +- current/dipole conversion +- saturation +- coil power +- diagnostic flags + +The optional C++ core should be a small pybind library, not a Basilisk external module, until there is a strong reason to use Basilisk's C++ plugin path. + +## External references used for the audit + +- Basilisk install notes: PyPI wheels are available via `pip install bsk`, and `bsk[examples]` adds optional example dependencies. +- Basilisk spacecraft module: native 6-DOF translational and rotational spacecraft dynamics. +- Basilisk WMM example/module: WMM magnetic field can be evaluated at spacecraft locations. +- Basilisk magnetometer module: TAM measurements in sensor frame. +- Basilisk MtbEffector module: commanded magnetic torque-bar dipoles become body torque. +- Basilisk Python modules: suitable for quick prototyping without C++/SWIG module rebuilds. diff --git a/docs/build_workflow.md b/docs/build_workflow.md new file mode 100644 index 0000000..9193425 --- /dev/null +++ b/docs/build_workflow.md @@ -0,0 +1,62 @@ +# Fast build workflow + +## What gets rebuilt + +| Change | Rebuild needed? | Command | +|---|---:|---| +| Edit Basilisk scenario Python | No | Run Python script again. | +| Edit plotting/comparison Python | No | Run plot/compare script again. | +| Edit `basilisk_adcs_adapter.py` Python controller | No | Run scenario again. | +| Edit `cpp_adcs_core/src/adcs_core.cpp` | Yes, small C++ core only | `cmake --build build --parallel` | +| Edit Basilisk source or true ExternalModules plugin | Yes, Basilisk rebuild | Avoid unless clearly necessary. | + +## Standard setup + +```powershell +cd C:\Users\chine\Documents\satellite_adcs_basilisk_project +python -m venv .venv +Set-ExecutionPolicy -Scope Process -ExecutionPolicy Bypass +.\.venv\Scripts\Activate.ps1 +python -m pip install --upgrade pip +python -m pip install -r requirements.txt +``` + +## Standard run loop + +```powershell +python .\basilisk_runner\scenario_huskysat2_detumble.py +python .\basilisk_runner\plot_results.py +python .\basilisk_runner\compare_reference_vs_basilisk.py +``` + +## Optional C++ ADCS core loop + +Configure once: + +```powershell +cmake -S cpp_adcs_core -B build -DCMAKE_BUILD_TYPE=Release +``` + +Rebuild after C++ ADCS changes: + +```powershell +cmake --build build --parallel +``` + +Run smoke test: + +```powershell +.\build\adcs_core_smoke.exe +``` + +If pybind is found, copy or keep the generated `adcs_core` module where Python can import it. The adapter will automatically use it if importable; otherwise it uses the Python fallback. + +## What not to do by default + +Do not make this the first normal path: + +```python +from Basilisk.ExternalModules import mtqDetumble +``` + +That path requires a Basilisk external C++ module build and links normal controller changes to the large Basilisk build/SWIG pipeline. diff --git a/docs/generated_file_manifest.json b/docs/generated_file_manifest.json new file mode 100644 index 0000000..17d3cb5 --- /dev/null +++ b/docs/generated_file_manifest.json @@ -0,0 +1,69 @@ +[ + ".gitignore", + ".vscode/tasks.json", + "README.md", + "archive/old_attempts/huskysat2_bsk/ExternalModules/mtqDetumble/mtqDetumble.cpp", + "archive/old_attempts/huskysat2_bsk/ExternalModules/mtqDetumble/mtqDetumble.h", + "archive/old_attempts/huskysat2_bsk/ExternalModules/mtqDetumble/mtqDetumble.i", + "archive/old_attempts/huskysat2_bsk/README.md", + "archive/old_attempts/huskysat2_bsk/fix.pypython", + "archive/old_attempts/huskysat2_bsk/fix2.pypython", + "archive/old_attempts/huskysat2_bsk/plot_results.py", + "archive/old_attempts/huskysat2_bsk/scenario_huskysat2_detumble.py", + "basilisk_runner/basilisk_adcs_adapter.py", + "basilisk_runner/compare_reference_vs_basilisk.py", + "basilisk_runner/output_data/.gitkeep", + "basilisk_runner/output_plots/.gitkeep", + "basilisk_runner/plot_results.py", + "basilisk_runner/scenario_huskysat2_detumble.py", + "basilisk_runner/scenario_huskysat2_minimal.py", + "basilisk_runner/scenario_huskysat2_pointing.py", + "cpp_adcs_core/CMakeLists.txt", + "cpp_adcs_core/bindings/pybind11_module.cpp", + "cpp_adcs_core/include/adcs/adcs_core.hpp", + "cpp_adcs_core/src/adcs_core.cpp", + "cpp_adcs_core/tests/test_adcs_core.cpp", + "docs/basilisk_capability_audit.md", + "docs/build_workflow.md", + "docs/interface_contract.md", + "docs/migration_plan.md", + "docs/source_inspection_report.md", + "docs/validation_plan.md", + "reference_standalone/adcs_output.csv", + "reference_standalone/original_project/.gitignore", + "reference_standalone/original_project/CMakeLists.txt", + "reference_standalone/original_project/FILE_GUIDE.md", + "reference_standalone/original_project/README_REFACTORED_SIM.txt", + "reference_standalone/original_project/README_WMM_UPDATE.txt", + "reference_standalone/original_project/SIMULATION_MAP.md", + "reference_standalone/original_project/adcs_output.csv", + "reference_standalone/original_project/include/frames.hpp", + "reference_standalone/original_project/include/math.hpp", + "reference_standalone/original_project/include/modules.hpp", + "reference_standalone/original_project/include/params.hpp", + "reference_standalone/original_project/include/quaternion.hpp", + "reference_standalone/original_project/include/rng.hpp", + "reference_standalone/original_project/include/sim_context.hpp", + "reference_standalone/original_project/include/wmm.hpp", + "reference_standalone/original_project/plot_all.py", + "reference_standalone/original_project/reference/README-WMM-COEFS.txt", + "reference_standalone/original_project/reference/WMM.COF", + "reference_standalone/original_project/reference/WMM2025.COF", + "reference_standalone/original_project/reference/WMM2025_TestValues.txt", + "reference_standalone/original_project/src/control.cpp", + "reference_standalone/original_project/src/disturbance.cpp", + "reference_standalone/original_project/src/main.cpp", + "reference_standalone/original_project/src/navigation.cpp", + "reference_standalone/original_project/src/satellite.cpp", + "reference_standalone/original_project/src/sensor.cpp", + "reference_standalone/original_project/src/wmm.cpp", + "reference_standalone/output_plots/body_rates.png", + "reference_standalone/output_plots/coil_power.png", + "reference_standalone/output_plots/currents.png", + "reference_standalone/output_plots/dipole.png", + "reference_standalone/output_plots/disturbances.png", + "reference_standalone/output_plots/mag_field.png", + "reference_standalone/output_plots/nav_euler.png", + "reference_standalone/output_plots/summary_plots.png", + "requirements.txt" +] \ No newline at end of file diff --git a/docs/interface_contract.md b/docs/interface_contract.md new file mode 100644 index 0000000..ef0d162 --- /dev/null +++ b/docs/interface_contract.md @@ -0,0 +1,178 @@ +# ADCS interface contract + +This contract defines the clean boundary between Basilisk and the project-specific ADCS core. + +The goal is that `cpp_adcs_core` does **not** depend on Basilisk. Python is responsible for converting Basilisk messages into this interface. + +## Function shape + +Conceptual Python-side call: + +```python +cmd = adcs_core.step( + time=t, + r_N=r_N, + v_N=v_N, + attitude=attitude, + omega_B=omega_B, + mag_B=mag_B, + sun_vec_B=sun_vec_B, + mode=mode, +) +``` + +The current C++ scaffold focuses on detumble and exposes the minimum useful subset: + +```text +omega_B_rad_s +mag_B_T +``` + +The larger interface below is the intended stable contract. + +## Inputs + +| Name | Type | Units | Frame | Description | Status | +|---|---:|---:|---|---|---| +| `time_s` | scalar | s | simulation clock | Time since scenario start. | Required | +| `r_N_m` | length-3 vector | m | inertial N | Spacecraft position relative to central body. | Planned | +| `v_N_m_s` | length-3 vector | m/s | inertial N | Spacecraft velocity relative to central body. | Planned | +| `attitude_type` | string | - | - | Attitude representation identifier, e.g. `MRP_BN` or `quat_IB`. | Required for full interface | +| `sigma_BN` | length-3 vector | dimensionless | B relative to N | Basilisk MRP attitude. | Preferred native Basilisk input | +| `q_IB` | length-4 vector | dimensionless | standalone convention | Scalar-first quaternion from standalone if needed. | Conversion-only | +| `omega_BN_B_rad_s` | length-3 vector | rad/s | body B | Body angular velocity relative to inertial frame, expressed in body frame. | Required | +| `mag_B_T` | length-3 vector | T | body B / sensor S if aligned | Magnetic field expressed in body frame. | Required | +| `sun_vec_B` | length-3 vector | unitless | body B | Unit vector from spacecraft to Sun, expressed in body frame. | Planned | +| `mode` | enum/string | - | - | Controller mode, e.g. `DETUMBLE`, `POINTING`, `SAFE`, `OFF`. | Planned | +| `config` | struct/dict | mixed | - | Controller gains, actuator limits, coil properties. | Required | + +## Outputs + +| Name | Type | Units | Frame | Description | +|---|---:|---:|---|---| +| `commanded_magnetic_dipole_B_Am2` | length-3 vector | A m^2 | body B / MTB array basis | Commanded magnetic dipole after saturation. | +| `commanded_coil_current_A` | length-3 vector | A | actuator axes | Coil current command after current and dipole limiting. | +| `commanded_control_torque_B_Nm` | length-3 vector | N m | body B | Diagnostic torque estimate `m x B`; Basilisk `MtbEffector` should apply the truth torque. | +| `coil_power_W` | length-3 vector | W | per axis | `I^2 R` coil power estimate. | +| `coil_power_total_W` | scalar | W | - | Sum of per-axis coil power. | +| `controller_mode` | string/int | - | - | Active controller mode. | +| `saturation_flags` | length-3 bool vector | - | per axis | True where command was saturated. | +| `valid` | bool | - | - | False if input was invalid or magnetic field was degenerate. | +| `diagnostics` | dict | mixed | - | Optional B-dot, raw dipole, gains, errors, etc. | + +## Units + +Use SI units only: + +- time: seconds +- distance: meters +- velocity: meters/second +- angular rate: radians/second +- magnetic field: Tesla +- dipole: A m^2 +- current: A +- torque: N m +- power: W + +## Frames and ordering + +Default vector ordering is always: + +```text +[x, y, z] +``` + +Basilisk-side native states: + +```text +r_BN_N position, inertial N +v_BN_N velocity, inertial N +sigma_BN MRP attitude of body B relative to inertial N +omega_BN_B angular velocity of B relative to N, expressed in B +``` + +Standalone-side reference states: + +```text +x,y,z, xdot,ydot,zdot, q0,q1,q2,q3, p,q,r +``` + +TODO: confirm exact standalone quaternion label convention before using `q_IB` for any control calculation. + +## Magnetic field convention + +Preferred controller input: + +```text +mag_B_T = magnetic field vector expressed in body frame B, Tesla +``` + +In the Basilisk detumble scenario, this comes from the magnetometer module with sensor frame aligned to body frame: + +```text +dcm_SB = identity +``` + +TODO: confirm whether Basilisk WMM `magField_N` sign and frame match the standalone `BfieldReference_eci_T` output. + +## Sign convention for magnetorquer torque + +Diagnostic torque convention: + +```text +torque_B_Nm = commanded_magnetic_dipole_B_Am2 x mag_B_T +``` + +Basilisk `MtbEffector` should be treated as the truth torque application path. The adapter's torque is for logging and sanity checks. + +TODO: confirm Basilisk `MtbEffector` internal sign against a one-step controlled test. + +## Saturation convention + +For each axis: + +```text +allowed_current = min(current_limit_A, dipole_limit_Am2 / dipole_gain_Am2_per_A) +raw_current = desired_dipole_Am2 / dipole_gain_Am2_per_A +commanded_current = clamp(raw_current, -allowed_current, +allowed_current) +commanded_dipole = commanded_current * dipole_gain +``` + +If a dipole gain or limit is nonpositive, command zero on that axis and set saturation/invalid diagnostic. + +## Mode convention + +Initial enum/string set: + +| Mode | Meaning | +|---|---| +| `OFF` | Zero actuator command. | +| `DETUMBLE` | Rate damping using magnetic field and body rate. | +| `POINTING` | Attitude/pointing control; not active in this scaffold. | +| `SAFE` | Conservative low-power or fault mode; not active in this scaffold. | + +TODO: replace with the actual HuskySat mode table. + +## Error handling + +The ADCS core should return a zero command and `valid=False` when: + +- any required vector is NaN or Inf +- magnetic field magnitude is below threshold +- required config values are invalid +- mode is unknown + +Python adapter should not crash a long Basilisk run for a single invalid controller input unless the failure indicates a scenario setup error. + +## Required TODOs before claiming equivalence + +```python +# TODO: replace with actual HuskySat inertia +# TODO: verify magnetorquer coil parameters +# TODO: confirm magnetic-field frame convention +# TODO: match initial angular velocity to standalone reference +# TODO: match orbit to standalone reference +# TODO: confirm whether Basilisk WMM output matches standalone WMM convention +# TODO: confirm Basilisk MtbEffector torque sign with one-step test +# TODO: decide whether standalone navigation scaffold should be replaced by simpleNav or a custom estimator +``` diff --git a/docs/migration_plan.md b/docs/migration_plan.md new file mode 100644 index 0000000..23c298e --- /dev/null +++ b/docs/migration_plan.md @@ -0,0 +1,170 @@ +# Migration plan + +## Milestone 0 — Preserve and document standalone reference + +Status: **done in this scaffold** + +- Clean copy of standalone source placed in `reference_standalone/original_project/`. +- Known-good generated CSV placed in `reference_standalone/adcs_output.csv`. +- Known-good plots placed in `reference_standalone/output_plots/`. +- Source inspection report written in `docs/source_inspection_report.md`. + +## Milestone 1 — Minimal Basilisk spacecraft/orbit/attitude simulation + +Status: **created, needs local run with installed `bsk`** + +Script: + +```text +basilisk_runner/scenario_huskysat2_minimal.py +``` + +Purpose: + +- verify pip-installed Basilisk imports +- verify VS Code/PowerShell workflow +- create spacecraft +- attach central gravity +- run one short 6-DOF simulation +- log position, velocity, MRP attitude, body rates +- generate first body-rate and attitude plots + +No WMM, magnetometer, magnetorquer, or custom C++ is required. + +## Milestone 2 — Add Basilisk WMM magnetic field + +Status: **created inside detumble scenario, needs local run** + +Script: + +```text +basilisk_runner/scenario_huskysat2_detumble.py +``` + +Actions: + +- add `magneticFieldWMM.MagneticFieldWMM` +- search installed Basilisk package for `WMM2025.COF` +- fall back to local `reference_standalone/original_project/reference/WMM2025.COF` +- log inertial magnetic field +- add magnetometer for body/sensor-frame magnetic field + +Validation: + +- compare magnetic field magnitude against standalone +- verify frame/sign convention before controller tuning + +## Milestone 3 — Add magnetic torque-bar application + +Status: **created, needs local run** + +Use: + +```python +from Basilisk.simulation import MtbEffector +``` + +The controller produces commanded dipole. Basilisk `MtbEffector` applies the torque to the spacecraft. + +TODO: + +- confirm `GtMatrix_B` row-major layout against current installed Basilisk version +- confirm field frame used by `MtbEffector` and logged WMM output + +## Milestone 4 — Add detumble control + +Status: **created in Python; optional C++ core created** + +Primary fast path: + +```text +basilisk_runner/basilisk_adcs_adapter.py +``` + +This defines a Python Basilisk module that: + +- reads `NavAttMsg` +- reads `TAMSensorMsg` +- computes rate-cross-field detumble command +- applies current and dipole saturation +- writes `MTBCmdMsg` +- stores diagnostic history for currents, power, and saturation flags + +Optional C++ fast path: + +```text +cpp_adcs_core/ +``` + +Builds independently of Basilisk and can expose `adcs_core.step(...)` through pybind11 when available. + +## Milestone 5 — Add pointing control + +Status: **scaffold only** + +Script: + +```text +basilisk_runner/scenario_huskysat2_pointing.py +``` + +Do not force pointing before detumble is stable. First decision after detumble: + +- If reaction wheels are used later, evaluate Basilisk `mrpFeedback` + RW effectors. +- If magnetorquer-only pointing is required, keep custom project-specific control because pure magnetic torque is underactuated instantaneously. + +## Milestone 6 — Add environmental disturbances + +Status: **planned** + +Candidate Basilisk native path: + +- SRP dynamic effector +- drag effector / atmosphere model +- gravity-gradient effector if appropriate +- `extForceTorque` for residual custom disturbance terms + +Do not tune disturbances before the minimal + WMM + MTB detumble chain is verified. + +## Milestone 7 — Compare against standalone reference + +Status: **comparison script created** + +Script: + +```text +basilisk_runner/compare_reference_vs_basilisk.py +``` + +Metrics: + +- initial angular speed magnitude +- final angular speed magnitude +- detumble time +- body-rate decay +- magnetic field magnitude/history +- commanded dipole behavior +- commanded current behavior +- coil power mean and peak +- disturbance torque magnitude if available +- numerical sanity checks + +## Milestone 8 — Clean up, test, VS Code workflow + +Status: **initial files created** + +Created: + +- `.gitignore` +- `.vscode/tasks.json` +- `requirements.txt` +- docs +- optional C++ core smoke test + +Next: + +- run locally with installed Basilisk +- record exact Basilisk version +- update TODOs in `interface_contract.md` +- add pytest for controller math and CSV sanity +- decide whether any true Basilisk plugin is justified later diff --git a/docs/source_inspection_report.md b/docs/source_inspection_report.md new file mode 100644 index 0000000..ed3facd --- /dev/null +++ b/docs/source_inspection_report.md @@ -0,0 +1,216 @@ +# Source inspection report + +## Uploaded standalone simulation + +Location inspected in the uploaded archive: + +```text +adcs_wmm2025_updated_cohesive/ +``` + +The standalone simulation is already organized as a small CMake C++17 executable: + +```text +CMakeLists.txt +include/ +src/ +reference/ +plot_all.py +adcs_output.csv +output_plots/ +``` + +### Build structure + +`CMakeLists.txt` builds one executable named `adcs` from: + +```text +src/main.cpp +src/satellite.cpp +src/control.cpp +src/sensor.cpp +src/navigation.cpp +src/disturbance.cpp +src/wmm.cpp +``` + +It uses local headers in `include/` and no visible external library dependency. + +### Standalone entry point + +Entry point: + +```text +src/main.cpp +``` + +Main loop order: + +1. Compute truth magnetic field. +2. Sample sensors. +3. Update navigation. +4. Update controller. +5. Log CSV row. +6. Advance truth state with RK4. +7. Normalize quaternion. + +### State definition + +The truth state is a 13-element vector: + +```text +[x, y, z, xdot, ydot, zdot, q0, q1, q2, q3, p, q, r] +``` + +Units: + +- position: m +- velocity: m/s +- quaternion: scalar-first inertial/body convention used internally by the standalone code +- body rates: rad/s + +### Dynamics files + +| File | Responsibility | +|---|---| +| `src/satellite.cpp` | Truth orbit/attitude derivatives, Earth constants, spacecraft parameters, magnetorquer dipole and coil power conversions. | +| `src/disturbance.cpp` | Atmosphere-relative drag, solar radiation pressure force, gravity-gradient torque, residual magnetic torque, optional aero/SRP torque offsets. | +| `src/wmm.cpp` | Embedded WMM2025 coefficient backend. | +| `src/sensor.cpp` | Magnetometer and gyro-like sampled measurements with fixed bias and white noise. | +| `src/navigation.cpp` | Lightweight estimator scaffold using gyro propagation and magnetic-field correction. | +| `src/control.cpp` | Placeholder detumble/rate-cross-field magnetorquer controller. | +| `src/main.cpp` | Simulation schedule, RK4 integration, CSV logging. | + +### WMM and magnetic field path + +The standalone magnetic field path is: + +```text +ECI position -> ECEF position -> geodetic lat/lon/alt -> WMM2025 NED -> ECEF -> ECI -> body +``` + +`reference/WMM2025.COF` and related files are included for traceability. The current runtime uses coefficients embedded in `src/wmm.cpp`. + +### Controller logic + +`src/control.cpp` is intentionally described by the standalone project as a placeholder controller. It computes a commanded current using a rate-cross-field damping law: + +```text +m_desired = K * (omega_body x B_body) +current = clamp(m_desired / dipole_gain) +dipole = current * dipole_gain +``` + +The custom logic worth preserving is the controller API, actuator saturation, current-to-dipole conversion, and coil power calculation. The full truth dynamics should not be ported blindly because Basilisk already provides the simulation plant. + +### Disturbance logic + +`src/disturbance.cpp` currently models: + +- atmosphere-relative drag force +- solar radiation pressure force with eclipse gating +- gravity-gradient torque +- residual magnetic dipole torque +- aero and SRP torque hooks through configurable center-of-pressure offsets + +The aero/SRP torque offsets are zero in the standalone run, so these are scaffolds rather than validated HuskySat-specific geometry values. + +### CSV output + +The known-good `adcs_output.csv` contains 70 columns: + +```text +t, x, y, z, xdot, ydot, zdot, q0, q1, q2, q3, p, q, r, +BBx, BBy, BBz, Bmx, Bmy, Bmz, BNx, BNy, BNz, +pqrm_x, pqrm_y, pqrm_z, pqrN_x, pqrN_y, pqrN_z, +ptpN_phi, ptpN_theta, ptpN_psi, qN0, qN1, qN2, qN3, +ix, iy, iz, mcmd_x, mcmd_y, mcmd_z, +pcoil_x, pcoil_y, pcoil_z, pcoil_total, +rho_kg_m3, vrel_m_s, sunlit, +Fdrag_x, Fdrag_y, Fdrag_z, Fsrp_x, Fsrp_y, Fsrp_z, +Taero_x, Taero_y, Taero_z, Tgg_x, Tgg_y, Tgg_z, +Tmag_x, Tmag_y, Tmag_z, Tsrp_x, Tsrp_y, Tsrp_z, +Tdist_x, Tdist_y, Tdist_z +``` + +The migration does not need to match this exact CSV schema. It should match equivalent physical metrics. + +### Plot behavior + +`plot_all.py` reads `adcs_output.csv` and creates: + +```text +body_rates.png +nav_euler.png +mag_field.png +currents.png +dipole.png +coil_power.png +disturbances.png +summary_plots.png +``` + +It also prints the reference metrics that were provided in the migration request. + +## Uploaded first Basilisk attempt + +Location inspected: + +```text +huskysat2_bsk/ +``` + +Important files: + +```text +scenario_huskysat2_detumble.py +plot_results.py +ExternalModules/mtqDetumble/mtqDetumble.cpp +ExternalModules/mtqDetumble/mtqDetumble.h +ExternalModules/mtqDetumble/mtqDetumble.i +fix.pypython +fix2.pypython +README.md +``` + +### What the old attempt did correctly + +It correctly identified useful Basilisk pieces: + +- `spacecraft.Spacecraft` +- `gravityEffector.GravBodyData` +- `magneticFieldWMM.MagneticFieldWMM` +- `simpleNav.SimpleNav` +- `magnetometer.Magnetometer` +- `MtbEffector.MtbEffector` +- Basilisk recorder messages + +### Why the old attempt caused slow workflow + +The old attempt imports: + +```python +from Basilisk.ExternalModules import mtqDetumble +``` + +The old README instructs copying `ExternalModules/` into the Basilisk source tree or building Basilisk using `--externalModules`. That makes ordinary controller edits depend on a Basilisk source rebuild/SWIG workflow. This is the path to avoid for normal development. + +### Hard-coded path issue + +The old attempt uses a machine-specific WMM path: + +```text +C:\Users\chine\Documents\basilisk\dist3\Basilisk\supportData\MagneticField\WMM2025.COF +``` + +The new runner avoids this by searching the installed `Basilisk` package support-data tree and falling back to the local standalone reference coefficient file if present. + +### What from the old attempt is retained + +The old attempt is archived under: + +```text +archive/old_attempts/huskysat2_bsk/ +``` + +It is retained as reference only. The new workflow does not require `Basilisk.ExternalModules.mtqDetumble`. diff --git a/docs/validation_plan.md b/docs/validation_plan.md new file mode 100644 index 0000000..13afa08 --- /dev/null +++ b/docs/validation_plan.md @@ -0,0 +1,80 @@ +# Validation plan + +## Principle + +Do not compare the old standalone 70-column CSV against Basilisk column-for-column. Basilisk produces message logs with different schema. Compare equivalent physical behavior. + +## Canonical Basilisk output + +The Basilisk runner writes canonical CSV files under: + +```text +basilisk_runner/output_data/ +``` + +Minimum useful columns: + +```text +time_s +r_N_x_m, r_N_y_m, r_N_z_m +v_N_x_m_s, v_N_y_m_s, v_N_z_m_s +sigma_BN_1, sigma_BN_2, sigma_BN_3 +omega_B_x_rad_s, omega_B_y_rad_s, omega_B_z_rad_s +omega_mag_rad_s +``` + +Detumble/magnetic columns: + +```text +B_N_x_T, B_N_y_T, B_N_z_T, B_N_mag_T +B_B_x_T, B_B_y_T, B_B_z_T, B_B_mag_T +mcmd_x_Am2, mcmd_y_Am2, mcmd_z_Am2 +ix_A, iy_A, iz_A +pcoil_x_W, pcoil_y_W, pcoil_z_W, pcoil_total_W +saturation_x, saturation_y, saturation_z +controller_valid +``` + +Disturbance/pointing columns are optional until implemented. + +## Metrics + +| Metric | Reference source | Basilisk source | Pass/fail logic | +|---|---|---|---| +| Initial angular speed | `sqrt(p^2+q^2+r^2)` at first reference row | `omega_mag_rad_s` first row | Should match configured initial condition closely. | +| Final angular speed | final reference row | final Basilisk row | Compare trend first; exact equality not expected. | +| Detumble time | first time below threshold | first time below threshold | Use thresholds such as 0.05, 0.02, 0.01 rad/s. | +| Body-rate decay | time history of `p,q,r` | time history of `omega_B_*` | Compare monotonic/trend and final magnitude. | +| Magnetic field magnitude | `sqrt(BBx^2+BBy^2+BBz^2)` | `B_B_mag_T` or `B_N_mag_T` | Should be same order and similar orbital trend after frame/epoch check. | +| Commanded dipole | `mcmd_*` | `mcmd_*_Am2` | Saturation behavior should match controller logic. | +| Commanded current | `ix,iy,iz` | `ix_A,iy_A,iz_A` | Verify current limits. | +| Coil power | `pcoil_total` | `pcoil_total_W` | Mean/peak should be comparable after controller match. | +| Disturbance torque | `Tdist_*` | disturbance columns | Only compare after disturbances are implemented. | +| Numerical sanity | all numeric columns | all numeric columns | No NaNs or infinities. | + +## Initial reference metrics from uploaded standalone run + +Known-good standalone summary: + +```text +Rows: 57943 +Cols: 70 +All numeric values finite?: True +Initial |w| [rad/s]: 0.8774964387392122 +Final |w| [rad/s]: 0.010041780947787082 +Mean coil power [W]: 0.7946818911022777 +Peak coil power [W]: 2.5212665406427224 +Mean drag force [N]: 2.0595744595556306e-07 +Mean SRP force [N]: 9.865491785096374e-08 +Mean total disturbance torque [N m]: 1.2038253703098527e-07 +``` + +## Test sequence + +1. Run `scenario_huskysat2_minimal.py` and confirm no NaN/Inf. +2. Run `scenario_huskysat2_detumble.py` with WMM and magnetometer but controller gain set to zero. Confirm field logging and no dynamics instability. +3. Enable Python detumble controller. Confirm rates decrease. +4. Build optional pybind C++ core. Confirm Python and C++ controller outputs match for the same input vectors. +5. Compare against standalone metrics. +6. Add disturbances only after the magnetic detumble loop is stable. +7. Re-run comparison after each subsystem is added. diff --git a/reference_standalone/original_project/.gitignore b/reference_standalone/original_project/.gitignore new file mode 100644 index 0000000..1e7598b --- /dev/null +++ b/reference_standalone/original_project/.gitignore @@ -0,0 +1,9 @@ +build/ +adcs_output.csv +output_plots/ +*.exe +*.obj +*.o +*.dll +*.pyc +__pycache__/ diff --git a/reference_standalone/original_project/CMakeLists.txt b/reference_standalone/original_project/CMakeLists.txt new file mode 100644 index 0000000..5a2ac62 --- /dev/null +++ b/reference_standalone/original_project/CMakeLists.txt @@ -0,0 +1,25 @@ +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) + +# 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 +) + +# All headers live in the local include folder. +target_include_directories(adcs PRIVATE include) + +# MSVC does not expose M_PI from unless this is defined before including cmath. +target_compile_definitions(adcs PRIVATE _USE_MATH_DEFINES) diff --git a/reference_standalone/original_project/FILE_GUIDE.md b/reference_standalone/original_project/FILE_GUIDE.md new file mode 100644 index 0000000..7de232d --- /dev/null +++ b/reference_standalone/original_project/FILE_GUIDE.md @@ -0,0 +1,94 @@ +# File-by-file guide to the refactored simulation + +## `CMakeLists.txt` +Builds one executable called `adcs` from the simulation source files. + +## `include/math.hpp` +Small vector and 3x3 matrix helpers. +Everything else depends on this file for basic math. + +## `include/quaternion.hpp` +Quaternion representation and conversions. +Used by both the truth attitude dynamics and the plotting / logging helpers. + +## `include/frames.hpp` +All coordinate-frame utilities live here. +That includes ECI/ECEF rotation, ECEF->geodetic conversion, and NED basis vectors. + +## `include/wmm.hpp` +Public interface for the WMM2025 magnetic model backend. + +## `include/sim_context.hpp` +Holds long-lived configuration and latched simulation signals. +Think of it as the shared notebook for the whole simulation. + +## `include/modules.hpp` +Declares the main simulation modules and the 13-state layout. +This is the file to read first if you want to see the architecture at a glance. + +## `include/params.hpp` +Contains the integrator time step. + +## `src/main.cpp` +This is the top-level simulation schedule. +It does the work in the same order each step: +1. compute truth field +2. sample sensors +3. update navigation +4. update controller +5. log everything +6. integrate truth state one RK4 step + +This is the best file to read if you want to understand the whole loop. + +## `src/satellite.cpp` +This file owns the truth physics. +It contains: +- spacecraft and Earth parameter initialization +- state extraction helpers +- WMM field conversion into the body frame +- pure orbit and attitude derivatives + +Nothing in this file should look like flight software scheduling. +It is just the truth model. + +## `src/sensor.cpp` +This file turns truth into measured values. +Bias is set once. Noise is re-drawn each sample. +Right now it provides only magnetometer and gyro style measurements. + +## `src/navigation.cpp` +This file holds the first estimator scaffold. +It propagates attitude with measured body rates and uses the expected magnetic field direction +for a lightweight correction. +It is still not a flight-ready estimator, but it no longer feeds the controller smoothed truth. + +## `src/control.cpp` +This file is a placeholder controller interface. +Right now it computes coil currents from a simple rate-cross-field damping law. +This is the file the magnetorquer team should eventually replace. + +## `src/disturbance.cpp` +This file adds non-control environmental effects. +It now models: +- atmosphere-relative drag force for a rotating atmosphere +- gravity-gradient torque +- residual magnetic dipole torque +- first-pass solar radiation pressure force with eclipse gating +- aerodynamic and SRP torque hooks through configurable center-of-pressure offsets + +The aerodynamic and SRP torque offsets are still set to zero by default until HuskySat-2-specific geometry data is available. + +## `src/wmm.cpp` +This is the embedded WMM2025 backend. +Its only job is to return local magnetic field components from geodetic position and decimal year. + +## `plot_all.py` +Quick-look plots for body rates, Euler angles, magnetic field, actuator commands, coil power, +and the disturbance force/torque breakdown. +It is not part of the physics loop. It only reads the CSV output. + +## `reference/` +Reference coefficient and test-value files for WMM2025. +These are included for traceability and cross-checking. +They are not the main runtime path because the current code embeds the coefficients in `wmm.cpp`. diff --git a/reference_standalone/original_project/README_REFACTORED_SIM.txt b/reference_standalone/original_project/README_REFACTORED_SIM.txt new file mode 100644 index 0000000..af614c9 --- /dev/null +++ b/reference_standalone/original_project/README_REFACTORED_SIM.txt @@ -0,0 +1,21 @@ +This package is the refactored version of the WMM2025 magnetorquer-only ADCS simulation. + +What changed in the refactor +- The truth dynamics were made pure. The derivative function now only computes state derivatives. +- Truth field update, sensor sampling, navigation, controller update, and logging now happen explicitly in main.cpp. +- The temporary controller is clearly separated from the physics so the magnetorquer team can replace it later. +- The disturbance model now includes atmosphere-relative drag, gravity-gradient torque, residual magnetic torque, and a first-pass solar radiation pressure force with eclipse gating. +- The sensor model now uses fixed per-axis bias plus fresh per-axis white noise each sample without a fake direct angle sensor. +- Navigation now uses a simple gyro-propagated quaternion estimate with magnetic-field correction instead of smoothed truth. + +What this simulation is for right now +- Closed-loop detumble development with magnetorquers only. +- WMM2025 field validation in a coherent orbit / attitude loop. +- Debugging controller interfaces before the real team control code is dropped in. + +What this simulation is not yet +- A flight-quality environment model. +- A flight estimator. +- A full actuator electronics model. +- A fully geometry-calibrated disturbance model. Aerodynamic and SRP torques still depend on center-of-pressure offsets that are currently left at zero until HuskySat-2-specific values are known. +- A final verification environment for hardware acceptance. diff --git a/reference_standalone/original_project/README_WMM_UPDATE.txt b/reference_standalone/original_project/README_WMM_UPDATE.txt new file mode 100644 index 0000000..add6fc1 --- /dev/null +++ b/reference_standalone/original_project/README_WMM_UPDATE.txt @@ -0,0 +1,42 @@ +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. + +Prerequisites +- Before running this simulation, make sure the following tools are installed and working on your system: +- CMake (used to configure and build the C++ code) +- https://cmake.org/download/ +- MSYS2 with UCRT64 toolchain (provides the C++ compiler on Windows) +- https://www.msys2.org/ + +- After installing MSYS2, open the UCRT64 terminal and run: +- pacman -S mingw-w64-ucrt-x86_64-gcc +- Python 3 +- https://www.python.org/downloads/ +- Required Python packages (used for plotting results): +- pip install numpy matplotlib pandas + +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. diff --git a/reference_standalone/original_project/SIMULATION_MAP.md b/reference_standalone/original_project/SIMULATION_MAP.md new file mode 100644 index 0000000..2e4e605 --- /dev/null +++ b/reference_standalone/original_project/SIMULATION_MAP.md @@ -0,0 +1,49 @@ +# How the simulation works + +This simulation is one closed loop with five stages. + +1. **Truth state** + The spacecraft truth state is the 13-element vector + `[position, velocity, quaternion, body rates]`. + Orbit is propagated in an inertial frame. Attitude is propagated with a quaternion. + +2. **Truth environment** + At the current truth state and time, the code computes the Earth magnetic field. + The path is: + ECI position -> ECEF position -> geodetic lat/lon/alt -> WMM2025 -> NED field -> ECEF -> ECI -> body frame. + The final body-frame field is stored in Tesla. + +3. **Sensors** + The truth magnetic field and truth body rates are sampled at the sensor rate. + The sensor model adds a fixed per-axis bias and fresh per-axis white noise. + These become the measured signals. + +4. **Navigation** + The navigation block propagates attitude with gyro measurements and uses the expected magnetic + field direction for a lightweight correction step. + This is still only a first estimator scaffold, but it no longer gives the controller direct angle truth. + +5. **Controller and actuator** + The controller reads the navigation estimates and outputs magnetorquer coil currents in amps. + Those currents define a magnetic dipole in the body frame. + The dipole crossed with the body-frame magnetic field gives the control torque. + +6. **Dynamics** + The truth state is advanced using RK4. + Translational dynamics include central gravity, atmosphere-relative drag, and first-pass solar radiation pressure. + Rotational dynamics include magnetorquer torque, gravity-gradient torque, residual magnetic torque, + optional aerodynamic/SRP offset torques, and rigid-body Euler equations. + +## Why the files are separated this way +- `main.cpp` runs the simulation schedule and logging. +- `satellite.cpp` owns truth physics and WMM field conversion. +- `sensor.cpp` turns truth into measured values. +- `navigation.cpp` turns measured values into smoothed estimates. +- `control.cpp` turns estimates into coil current commands. +- `disturbance.cpp` adds non-control environment forces and torques. +- `wmm.cpp` is only the magnetic field backend. + +## Current limitations +- The controller is still a placeholder detumble law. +- The navigation block is still only a first estimator scaffold, not a flight filter. +- Aerodynamic and solar-pressure torques still need HuskySat-2 center-of-pressure data before they should be trusted as non-zero hardware predictions. diff --git a/reference_standalone/original_project/include/frames.hpp b/reference_standalone/original_project/include/frames.hpp new file mode 100644 index 0000000..96c9488 --- /dev/null +++ b/reference_standalone/original_project/include/frames.hpp @@ -0,0 +1,119 @@ +#pragma once +#include "math.hpp" +#include + +// 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; +} diff --git a/reference_standalone/original_project/include/math.hpp b/reference_standalone/original_project/include/math.hpp new file mode 100644 index 0000000..2452f4e --- /dev/null +++ b/reference_standalone/original_project/include/math.hpp @@ -0,0 +1,157 @@ +#pragma once +#include +#include +#include + +// 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; +} diff --git a/reference_standalone/original_project/include/modules.hpp b/reference_standalone/original_project/include/modules.hpp new file mode 100644 index 0000000..6448068 --- /dev/null +++ b/reference_standalone/original_project/include/modules.hpp @@ -0,0 +1,94 @@ +#pragma once +#include "sim_context.hpp" +#include + +// 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; + +struct DisturbanceBreakdown { + double density_kg_m3{0.0}; + double relativeSpeed_m_s{0.0}; + bool sunlit{true}; + + Vec3 dragForce_eci_N{0.0, 0.0, 0.0}; + Vec3 solarRadiationForce_eci_N{0.0, 0.0, 0.0}; + + Vec3 aerodynamicTorque_body_Nm{0.0, 0.0, 0.0}; + Vec3 gravityGradientTorque_body_Nm{0.0, 0.0, 0.0}; + Vec3 residualMagneticTorque_body_Nm{0.0, 0.0, 0.0}; + Vec3 solarRadiationTorque_body_Nm{0.0, 0.0, 0.0}; + + Vec3 totalForce_eci_N() const { + return dragForce_eci_N + solarRadiationForce_eci_N; + } + + Vec3 totalTorque_body_Nm() const { + return aerodynamicTorque_body_Nm + + gravityGradientTorque_body_Nm + + residualMagneticTorque_body_Nm + + solarRadiationTorque_body_Nm; + } +}; + +// ----- Setup ----- +// Fill the context with constant model data before the first time step. +void initialize_simulation(SimContext& ctx); + +// ----- Truth-state helpers ----- +Quat state_quaternion(const State13& state); +Vec3 state_body_rates_rad_s(const State13& state); +Vec3 state_euler321_rad(const State13& state); + +// ----- Environment and dynamics ----- +// Compute the truth magnetic field in the inertial frame from the current truth state. +Vec3 truth_magnetic_field_eci_T(double t_s, const State13& state, const SimContext& ctx); + +// Compute the truth magnetic field in the body frame from the current truth state. +Vec3 truth_magnetic_field_body_T(double t_s, const State13& state, const SimContext& ctx); + +// Pure orbit/attitude derivative. This function reads state, environment, and actuator command, +// and returns only the time derivative of the 13-state. +State13 satellite_derivatives(double t_s, const State13& state, const SimContext& ctx); + +// ----- Sensor and navigation ----- +// Take the truth state and truth magnetic field and create measured signals. +void sensor_update_from_truth(const State13& truth_state, const Vec3& Btruth_body_T, SimContext& ctx); + +// Turn measured signals into the current navigation estimate. +void navigation_update(SimContext& ctx); + +// ----- Controller ----- +// Temporary magnetorquer-only detumble controller. +// This will be replaced when the magnetorquer team drops in the real control law. +void control_compute(const SimContext& ctx, Vec3& current_cmd_A); + +// ----- Actuator helpers ----- +// Convert the held current command into the magnetic dipole and coil power used this step. +Vec3 magnetorquer_dipole_body_Am2(const SimContext& ctx); +Vec3 magnetorquer_coil_power_W(const SimContext& ctx); +double magnetorquer_total_coil_power_W(const SimContext& ctx); + +// ----- Disturbance model ----- +// Returns the named disturbance contributors at the current truth state. +void disturbance_summary(double t_s, + double altitude_m, + const Vec3& pos_eci_m, + const Vec3& vel_eci_m_s, + const Quat& q_ib, + const Vec3& B_body_T, + const SimContext& ctx, + DisturbanceBreakdown& summary); + +// Returns the total non-control force and torque in the current state. +void disturbance(double t_s, + double altitude_m, + const Vec3& pos_eci_m, + const Vec3& vel_eci_m_s, + const Quat& q_ib, + const Vec3& B_body_T, + const SimContext& ctx, + Vec3& disturbanceForce_eci_N, + Vec3& disturbanceTorque_body_Nm); + +double density_kg_m3(double altitude_m); diff --git a/reference_standalone/original_project/include/params.hpp b/reference_standalone/original_project/include/params.hpp new file mode 100644 index 0000000..ab9366e --- /dev/null +++ b/reference_standalone/original_project/include/params.hpp @@ -0,0 +1,6 @@ +#pragma once + +namespace SimParams { + // Integration step used by the RK4 solver. + constexpr double timestep = 0.1; +} diff --git a/reference_standalone/original_project/include/quaternion.hpp b/reference_standalone/original_project/include/quaternion.hpp new file mode 100644 index 0000000..cc96853 --- /dev/null +++ b/reference_standalone/original_project/include/quaternion.hpp @@ -0,0 +1,103 @@ +#pragma once +#include "math.hpp" +#include + +// Scalar-first quaternion. +// q0 is the scalar term, q1/q2/q3 are the vector terms. +struct Quat { + double q0{1.0}; + double q1{0.0}; + double q2{0.0}; + double q3{0.0}; +}; + +// Keep a quaternion on the unit sphere. +inline Quat normalize(const Quat& q) { + const double n = std::sqrt(q.q0 * q.q0 + q.q1 * q.q1 + q.q2 * q.q2 + q.q3 * q.q3); + if (n == 0.0) { + return {1.0, 0.0, 0.0, 0.0}; + } + return {q.q0 / n, q.q1 / n, q.q2 / n, q.q3 / n}; +} + +// Rotation matrix that maps a vector from body coordinates to inertial coordinates. +inline Mat3 TIBquat(const Quat& q_) { + const Quat q = q_; + const double q0 = q.q0; + const double q1 = q.q1; + const double q2 = q.q2; + const double q3 = q.q3; + + const double q0s = q0 * q0; + const double q1s = q1 * q1; + const double q2s = q2 * q2; + const double q3s = q3 * q3; + + Mat3 R{}; + R[0][0] = q0s + q1s - q2s - q3s; + R[0][1] = 2.0 * (q1 * q2 - q0 * q3); + R[0][2] = 2.0 * (q0 * q2 + q1 * q3); + + R[1][0] = 2.0 * (q1 * q2 + q0 * q3); + R[1][1] = q0s - q1s + q2s - q3s; + R[1][2] = 2.0 * (q2 * q3 - q0 * q1); + + R[2][0] = 2.0 * (q1 * q3 - q0 * q2); + R[2][1] = 2.0 * (q0 * q1 + q2 * q3); + R[2][2] = q0s - q1s - q2s + q3s; + return R; +} + +// Convert 3-2-1 Euler angles to a quaternion. +inline Quat euler321_to_quat(const Vec3& ptp) { + const double phi = ptp.x; + const double theta = ptp.y; + const double psi = ptp.z; + + const double q0 = std::cos(phi / 2.0) * std::cos(theta / 2.0) * std::cos(psi / 2.0) + + std::sin(phi / 2.0) * std::sin(theta / 2.0) * std::sin(psi / 2.0); + const double q1 = std::sin(phi / 2.0) * std::cos(theta / 2.0) * std::cos(psi / 2.0) + - std::cos(phi / 2.0) * std::sin(theta / 2.0) * std::sin(psi / 2.0); + const double q2 = std::cos(phi / 2.0) * std::sin(theta / 2.0) * std::cos(psi / 2.0) + + std::sin(phi / 2.0) * std::cos(theta / 2.0) * std::sin(psi / 2.0); + const double q3 = std::cos(phi / 2.0) * std::cos(theta / 2.0) * std::sin(psi / 2.0) + - std::sin(phi / 2.0) * std::sin(theta / 2.0) * std::cos(psi / 2.0); + + return {q0, q1, q2, q3}; +} + +// Convert a quaternion back to 3-2-1 Euler angles. +inline Vec3 quat_to_euler321(const Quat& q) { + const double q0 = q.q0; + const double q1 = q.q1; + const double q2 = q.q2; + const double q3 = q.q3; + + const double phi = std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + double arg = 2.0 * (q0 * q2 - q3 * q1); + if (arg > 1.0) { + arg = 1.0; + } + if (arg < -1.0) { + arg = -1.0; + } + + const double theta = std::asin(arg); + const double psi = std::atan2(2.0 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2 * q2 + q3 * q3)); + return {phi, theta, psi}; +} + +// Standard rigid-body quaternion kinematics for body rates [p, q, r]. +inline Quat quat_derivative(const Quat& q, const Vec3& omega_body) { + const double p = omega_body.x; + const double qv = omega_body.y; + const double r = omega_body.z; + + Quat qdot{}; + qdot.q0 = -0.5 * ( p * q.q1 + qv * q.q2 + r * q.q3 ); + qdot.q1 = 0.5 * ( p * q.q0 + r * q.q2 - qv * q.q3 ); + qdot.q2 = 0.5 * ( qv * q.q0 - r * q.q1 + p * q.q3 ); + qdot.q3 = 0.5 * ( r * q.q0 + qv * q.q1 - p * q.q2 ); + return qdot; +} diff --git a/reference_standalone/original_project/include/rng.hpp b/reference_standalone/original_project/include/rng.hpp new file mode 100644 index 0000000..faf1f88 --- /dev/null +++ b/reference_standalone/original_project/include/rng.hpp @@ -0,0 +1,20 @@ +#pragma once +#include + +// Small wrapper around the C++ random generator. +// The default fixed seed makes runs repeatable, which is handy for debugging. +struct Rng { + std::mt19937_64 gen; + std::uniform_real_distribution unif; + + explicit Rng(uint64_t seed = 1) : gen(seed), unif(0.0, 1.0) {} + + double rand01() { + return unif(gen); + } + + // Match the common MATLAB pattern (2*rand()-1). + double rand_m11() { + return 2.0 * rand01() - 1.0; + } +}; diff --git a/reference_standalone/original_project/include/sim_context.hpp b/reference_standalone/original_project/include/sim_context.hpp new file mode 100644 index 0000000..102d2ff --- /dev/null +++ b/reference_standalone/original_project/include/sim_context.hpp @@ -0,0 +1,94 @@ +#pragma once +#include "math.hpp" +#include "quaternion.hpp" +#include "rng.hpp" + +// This struct holds the long-lived simulation configuration and latched signals. +// The goal is not to mimic flight software structure exactly. +// The goal is to keep one clear place for constants, truth values, measurements, +// nav estimates, and actuator commands so the rest of the code is easy to follow. +struct SimContext { + // ----- Planet model ----- + // Earth constants used by the orbit and magnetic field models. + double earthRadius_m{6.371e6}; + double earthMass_kg{5.972e24}; + double gravConst_SI{6.67e-11}; + double mu_m3_s2{0.0}; + + // Earth rotation for ECI <-> ECEF conversion. + double earthRotationRate_rad_s{7.2921150e-5}; + double greenwichAngle0_rad{0.0}; + + // Start date for WMM. One orbit is only a few hours, so the decimal year hardly changes, + // but the code still carries time correctly. + double decimalYear0{2026.0}; + + // ----- Spacecraft geometry and mass ----- + double mass_kg{2.6}; + double lx_m{0.10}; + double ly_m{0.10}; + double lz_m{0.20}; + double maxArea_m2{0.0}; + double maxMomentArm_m{0.0}; + double dragCoeff{1.0}; + double solarRadiationPressure_N_m2{4.56e-6}; + double solarReflectivityCoeff{1.3}; + Vec3 residualDipole_body_Am2{0.0, 0.0, 0.0}; + Vec3 aeroCpOffset_body_m{0.0, 0.0, 0.0}; + Vec3 srpCpOffset_body_m{0.0, 0.0, 0.0}; + + // Principal inertia model for the simple box spacecraft. + Mat3 inertia_body_kgm2{}; + Mat3 inertiaInv_body_kgm2{}; + + // ----- Actuator model ----- + // HuskySat-2 hybrid magnetorquer setup used by the current realism pass: + // X/Y are CubeSpace CR0002 rods and Z is an EXA MT01 on the long axis. + Vec3 mtqDipoleGain_Am2_A{0.0, 0.0, 0.0}; + Vec3 mtqResistance_Ohm{0.0, 0.0, 0.0}; + Vec3 mtqCurrentLimit_A{0.0, 0.0, 0.0}; + Vec3 mtqDipoleLimit_Am2{0.0, 0.0, 0.0}; + Vec3 commandedCurrent_A{0.0, 0.0, 0.0}; + + // ----- Sensor model ----- + // Sensor sample period. Truth is continuous; measurements are discrete. + double sensorPeriod_s{1.0}; + bool sensorModelInitialized{false}; + + // One fixed bias per axis plus fresh white noise each sample. + Vec3 magBias_T{0.0, 0.0, 0.0}; + Vec3 gyroBias_rad_s{0.0, 0.0, 0.0}; + + Vec3 magNoise_T{0.0, 0.0, 0.0}; + Vec3 gyroNoise_rad_s{0.0, 0.0, 0.0}; + + // ----- Navigation filter ----- + // This is a first estimator scaffold, not a flight-ready navigation solution. + // It propagates attitude with gyro measurements and uses the expected magnetic + // field direction for a lightweight correction step. + bool navInitialized{false}; + double navBlend{0.3}; + double navAttitudeCorrectionGain{0.8}; + double navBiasCorrectionGain{0.02}; + + Vec3 BfieldNav_T{0.0, 0.0, 0.0}; + Vec3 BfieldNavPrev_T{0.0, 0.0, 0.0}; + Vec3 pqrNav_rad_s{0.0, 0.0, 0.0}; + Vec3 pqrNavPrev_rad_s{0.0, 0.0, 0.0}; + Vec3 ptpNav_rad{0.0, 0.0, 0.0}; + Vec3 ptpNavPrev_rad{0.0, 0.0, 0.0}; + Vec3 BdotNav_T_s{0.0, 0.0, 0.0}; + Quat qNav_IB{}; + Vec3 gyroBiasNav_rad_s{0.0, 0.0, 0.0}; + + // ----- Latched truth and measured signals ----- + Vec3 BfieldReference_eci_T{0.0, 0.0, 0.0}; + Vec3 BfieldTruth_body_T{0.0, 0.0, 0.0}; + Vec3 BfieldMeasured_body_T{0.0, 0.0, 0.0}; + Vec3 pqrMeasured_rad_s{0.0, 0.0, 0.0}; + + // Random number generator used by the sensor noise model. + Rng rng; + + explicit SimContext(uint64_t seed = 1) : rng(seed) {} +}; diff --git a/reference_standalone/original_project/include/wmm.hpp b/reference_standalone/original_project/include/wmm.hpp new file mode 100644 index 0000000..0c6e449 --- /dev/null +++ b/reference_standalone/original_project/include/wmm.hpp @@ -0,0 +1,20 @@ +#pragma once +#include "math.hpp" + +// Compute the WMM2025 magnetic field at a geodetic point. +// Inputs: +// latitude_deg - geodetic latitude in degrees +// longitude_deg - geodetic longitude in degrees +// altitude_km - altitude above the WGS84 ellipsoid in kilometers +// decimal_year - decimal year, valid from 2025.0 through 2030.0 +// Outputs: +// X_nT - north component in nanoTesla +// Y_nT - east component in nanoTesla +// Z_nT - down component in nanoTesla +void wmm2025_geodetic_ned_nT(double latitude_deg, + double longitude_deg, + double altitude_km, + double decimal_year, + double& X_nT, + double& Y_nT, + double& Z_nT); diff --git a/reference_standalone/original_project/plot_all.py b/reference_standalone/original_project/plot_all.py new file mode 100644 index 0000000..31b39a4 --- /dev/null +++ b/reference_standalone/original_project/plot_all.py @@ -0,0 +1,225 @@ +from pathlib import Path + +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + + +OUTPUT_DIR = Path("output_plots") +OUTPUT_DIR.mkdir(exist_ok=True) + + +def mag3(x, y, z): + x = np.asarray(x, dtype=float) + y = np.asarray(y, dtype=float) + z = np.asarray(z, dtype=float) + return np.sqrt(x * x + y * y + z * z) + + +def save_current_figure(name): + path = OUTPUT_DIR / name + plt.tight_layout() + plt.savefig(path, dpi=180) + plt.close() + return path + + +df = pd.read_csv("adcs_output.csv") +t = df["t"].to_numpy() + +numeric = df.select_dtypes(include=[np.number]) +all_finite = np.isfinite(numeric.to_numpy(dtype=float)).all() + +wmag = mag3(df["p"], df["q"], df["r"]) +print("Rows:", len(df), "Cols:", len(df.columns)) +print("All numeric values finite?:", bool(all_finite)) +print("Initial |w| [rad/s]:", float(wmag[0])) +print("Final |w| [rad/s]:", float(wmag[-1])) +print("Mean coil power [W]:", float(df["pcoil_total"].mean())) +print("Peak coil power [W]:", float(df["pcoil_total"].max())) + + +plt.figure(figsize=(9, 4)) +plt.plot(t, df["p"], label="p") +plt.plot(t, df["q"], label="q") +plt.plot(t, df["r"], label="r") +plt.plot(t, wmag, label="|w|", color="black", linewidth=2, alpha=0.8) +plt.xlabel("Time [s]") +plt.ylabel("Rate [rad/s]") +plt.title("Body Rates") +plt.legend() +plt.grid(True) +body_rates_path = save_current_figure("body_rates.png") + + +if all(c in df.columns for c in ["ptpN_phi", "ptpN_theta", "ptpN_psi"]): + plt.figure(figsize=(9, 4)) + plt.plot(t, df["ptpN_phi"], label="phi") + plt.plot(t, df["ptpN_theta"], label="theta") + plt.plot(t, df["ptpN_psi"], label="psi") + plt.xlabel("Time [s]") + plt.ylabel("Angle [rad]") + plt.title("Navigation Euler Angles") + plt.legend() + plt.grid(True) + nav_euler_path = save_current_figure("nav_euler.png") +else: + nav_euler_path = None + + +B_truth = mag3(df["BBx"], df["BBy"], df["BBz"]) +B_meas = mag3(df["Bmx"], df["Bmy"], df["Bmz"]) +B_nav = mag3(df["BNx"], df["BNy"], df["BNz"]) + +plt.figure(figsize=(9, 4)) +plt.plot(t, B_truth, label="|B| truth") +plt.plot(t, B_meas, label="|B| meas", alpha=0.6) +plt.plot(t, B_nav, label="|B| nav", linewidth=2) +plt.xlabel("Time [s]") +plt.ylabel("|B| [T]") +plt.title("Magnetic Field Magnitude") +plt.legend() +plt.grid(True) +mag_field_path = save_current_figure("mag_field.png") + + +plt.figure(figsize=(9, 4)) +plt.plot(t, df["ix"], label="ix") +plt.plot(t, df["iy"], label="iy") +plt.plot(t, df["iz"], label="iz") +plt.xlabel("Time [s]") +plt.ylabel("Current [A]") +plt.title("Magnetorquer Currents") +plt.legend() +plt.grid(True) +currents_path = save_current_figure("currents.png") + + +plt.figure(figsize=(9, 4)) +plt.plot(t, df["mcmd_x"], label="mx") +plt.plot(t, df["mcmd_y"], label="my") +plt.plot(t, df["mcmd_z"], label="mz") +plt.xlabel("Time [s]") +plt.ylabel("Dipole [A m^2]") +plt.title("Magnetorquer Dipole Commands") +plt.legend() +plt.grid(True) +dipole_path = save_current_figure("dipole.png") + + +plt.figure(figsize=(9, 4)) +plt.plot(t, df["pcoil_x"], label="Px coil (CR0002)") +plt.plot(t, df["pcoil_y"], label="Py coil (CR0002)") +plt.plot(t, df["pcoil_z"], label="Pz coil (MT01)") +plt.plot(t, df["pcoil_total"], label="Ptotal coil", linewidth=2, color="black") +plt.xlabel("Time [s]") +plt.ylabel("Power [W]") +plt.title("Hybrid Coil Power") +plt.legend() +plt.grid(True) +coil_power_path = save_current_figure("coil_power.png") + + +if all( + c in df.columns + for c in [ + "Fdrag_x", + "Fdrag_y", + "Fdrag_z", + "Fsrp_x", + "Fsrp_y", + "Fsrp_z", + "Tgg_x", + "Tgg_y", + "Tgg_z", + "Tmag_x", + "Tmag_y", + "Tmag_z", + "Tsrp_x", + "Tsrp_y", + "Tsrp_z", + "Tdist_x", + "Tdist_y", + "Tdist_z", + ] +): + F_drag = mag3(df["Fdrag_x"], df["Fdrag_y"], df["Fdrag_z"]) + F_srp = mag3(df["Fsrp_x"], df["Fsrp_y"], df["Fsrp_z"]) + T_gg = mag3(df["Tgg_x"], df["Tgg_y"], df["Tgg_z"]) + T_mag = mag3(df["Tmag_x"], df["Tmag_y"], df["Tmag_z"]) + T_srp = mag3(df["Tsrp_x"], df["Tsrp_y"], df["Tsrp_z"]) + T_total = mag3(df["Tdist_x"], df["Tdist_y"], df["Tdist_z"]) + + print("Mean drag force [N]:", float(F_drag.mean())) + print("Mean SRP force [N]:", float(F_srp.mean())) + print("Mean total disturbance torque [N m]:", float(T_total.mean())) + + fig, axes = plt.subplots(2, 1, figsize=(9, 7), sharex=True) + + axes[0].plot(t, F_drag, label="|Fdrag|") + axes[0].plot(t, F_srp, label="|Fsrp|") + axes[0].set_ylabel("Force [N]") + axes[0].set_title("Environment Disturbance Forces") + axes[0].grid(True) + axes[0].legend() + + for series, label, color, linewidth in [ + (T_gg, "|Tgg|", None, None), + (T_mag, "|Tresidual mag|", None, None), + (T_srp, "|Tsrp|", None, None), + (T_total, "|Tdist total|", "black", 2), + ]: + if float(np.max(series)) > 0.0: + axes[1].semilogy( + t, + np.maximum(series, 1e-18), + label=label, + color=color, + linewidth=linewidth, + ) + axes[1].set_xlabel("Time [s]") + axes[1].set_ylabel("Torque [N m]") + axes[1].set_title("Environment Disturbance Torques") + axes[1].grid(True) + axes[1].legend() + + disturbance_path = save_current_figure("disturbances.png") +else: + disturbance_path = None + + +summary_paths = [ + ("body_rates", body_rates_path), + ("nav_euler", nav_euler_path), + ("mag_field", mag_field_path), + ("currents", currents_path), + ("dipole", dipole_path), + ("coil_power", coil_power_path), + ("disturbances", disturbance_path), +] + +print("Saved plots:") +for label, path in summary_paths: + if path is not None: + print(f" {label}: {path}") + +existing_paths = [path for _, path in summary_paths if path is not None] +if existing_paths: + ncols = 2 + nrows = int(np.ceil(len(existing_paths) / ncols)) + fig, axes = plt.subplots(nrows, ncols, figsize=(14, 4.8 * nrows)) + axes = np.atleast_1d(axes).ravel() + + for ax, (label, path) in zip(axes, [(label, path) for label, path in summary_paths if path is not None]): + ax.imshow(plt.imread(path)) + ax.set_title(label.replace("_", " ").title()) + ax.axis("off") + + for ax in axes[len(existing_paths):]: + ax.axis("off") + + summary_path = OUTPUT_DIR / "summary_plots.png" + plt.tight_layout() + plt.savefig(summary_path, dpi=180) + plt.close() + print(f" summary: {summary_path}") diff --git a/reference_standalone/original_project/reference/README-WMM-COEFS.txt b/reference_standalone/original_project/reference/README-WMM-COEFS.txt new file mode 100644 index 0000000..6ff7769 --- /dev/null +++ b/reference_standalone/original_project/reference/README-WMM-COEFS.txt @@ -0,0 +1,70 @@ +World Magnetic Model WMM2025 +======================================================== +Date December 17, 2024 + +WMM.COF WMM2025 Coefficients file + (Replace old WMM.COF (WMM2020 + or WMM2015) file with this) + +1. Installation Instructions +========================== + +WMM2025 GUI +----------- + +Go to installed directory, find WMM.COF and remove it. +Replace it with the new WMM.COF. + + +WMM_Linux and WMM_Windows C software +------------------------------------ + +For the version <= WMM2020, replace the WMM.COF file in the "bin" directory with the +new provided WMM.COF file + + +Your own software +----------------- +Depending on your installation, find the coefficient +file, WMM.COF (unless renamed). Replace it with the new +WMM.COF file (renaming it appropriately if necessary). + +If the coefficients are embedded in your software, you +may need to embed the new coefficients. + +2. Installation Verification +============================ + +To confirm you are using the correct WMM.COF file open +it and verify that the header is: + + 2025.0 WMM-2025 11/13/2024 + +To assist in confirming that the installation of the new +coefficient file is correct we provide a set of test +values in this package. Here are a few as an example: + +Date HAE Lat Long Decl Incl H X Y Z F Ddot Idot Hdot Xdot Ydot Zdot Fdot +2020.0 66 14 143 0.12 13.08 34916.9 34916.8 70.2 8114.9 35847.5 -0.1 -0.1 29.6 29.6 -41.4 -46.4 18.3 +2020.0 18 0 21 1.05 -26.46 29316.1 29311.2 536.0 -14589.0 32745.6 0.1 0.1 0.0 -0.9 51.2 54.5 -24.3 +2020.5 6 -36 -137 20.16 -52.21 25511.4 23948.6 8791.9 -32897.6 41630.3 0.0 0.0 -21.6 -21.6 -3.8 65.4 -64.9 +2020.5 63 26 81 0.43 40.84 34738.7 34737.7 259.2 30023.4 45914.9 0.0 0.1 5.9 5.9 2.4 128.2 88.3 + +Where HAE is height above WGS-84 ellipsoid. + + + + +Model Software Support +====================== + +* National Centers for Environmental Information (NCEI) +* E/NE42 325 Broadway +* Boulder, CO 80305 USA +* Attn: Manoj Nair or Arnaud Chulliat +* Phone: (303) 497-4642 or -6522 +* Email: geomag.models@noaa.gov +For more details about the World Magnetic Model visit +http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml + + diff --git a/reference_standalone/original_project/reference/WMM.COF b/reference_standalone/original_project/reference/WMM.COF new file mode 100644 index 0000000..098c579 --- /dev/null +++ b/reference_standalone/original_project/reference/WMM.COF @@ -0,0 +1,93 @@ + 2025.0 WMM-2025 11/13/2024 + 1 0 -29351.8 0.0 12.0 0.0 + 1 1 -1410.8 4545.4 9.7 -21.5 + 2 0 -2556.6 0.0 -11.6 0.0 + 2 1 2951.1 -3133.6 -5.2 -27.7 + 2 2 1649.3 -815.1 -8.0 -12.1 + 3 0 1361.0 0.0 -1.3 0.0 + 3 1 -2404.1 -56.6 -4.2 4.0 + 3 2 1243.8 237.5 0.4 -0.3 + 3 3 453.6 -549.5 -15.6 -4.1 + 4 0 895.0 0.0 -1.6 0.0 + 4 1 799.5 278.6 -2.4 -1.1 + 4 2 55.7 -133.9 -6.0 4.1 + 4 3 -281.1 212.0 5.6 1.6 + 4 4 12.1 -375.6 -7.0 -4.4 + 5 0 -233.2 0.0 0.6 0.0 + 5 1 368.9 45.4 1.4 -0.5 + 5 2 187.2 220.2 0.0 2.2 + 5 3 -138.7 -122.9 0.6 0.4 + 5 4 -142.0 43.0 2.2 1.7 + 5 5 20.9 106.1 0.9 1.9 + 6 0 64.4 0.0 -0.2 0.0 + 6 1 63.8 -18.4 -0.4 0.3 + 6 2 76.9 16.8 0.9 -1.6 + 6 3 -115.7 48.8 1.2 -0.4 + 6 4 -40.9 -59.8 -0.9 0.9 + 6 5 14.9 10.9 0.3 0.7 + 6 6 -60.7 72.7 0.9 0.9 + 7 0 79.5 0.0 -0.0 0.0 + 7 1 -77.0 -48.9 -0.1 0.6 + 7 2 -8.8 -14.4 -0.1 0.5 + 7 3 59.3 -1.0 0.5 -0.8 + 7 4 15.8 23.4 -0.1 0.0 + 7 5 2.5 -7.4 -0.8 -1.0 + 7 6 -11.1 -25.1 -0.8 0.6 + 7 7 14.2 -2.3 0.8 -0.2 + 8 0 23.2 0.0 -0.1 0.0 + 8 1 10.8 7.1 0.2 -0.2 + 8 2 -17.5 -12.6 0.0 0.5 + 8 3 2.0 11.4 0.5 -0.4 + 8 4 -21.7 -9.7 -0.1 0.4 + 8 5 16.9 12.7 0.3 -0.5 + 8 6 15.0 0.7 0.2 -0.6 + 8 7 -16.8 -5.2 -0.0 0.3 + 8 8 0.9 3.9 0.2 0.2 + 9 0 4.6 0.0 -0.0 0.0 + 9 1 7.8 -24.8 -0.1 -0.3 + 9 2 3.0 12.2 0.1 0.3 + 9 3 -0.2 8.3 0.3 -0.3 + 9 4 -2.5 -3.3 -0.3 0.3 + 9 5 -13.1 -5.2 0.0 0.2 + 9 6 2.4 7.2 0.3 -0.1 + 9 7 8.6 -0.6 -0.1 -0.2 + 9 8 -8.7 0.8 0.1 0.4 + 9 9 -12.9 10.0 -0.1 0.1 + 10 0 -1.3 0.0 0.1 0.0 + 10 1 -6.4 3.3 0.0 0.0 + 10 2 0.2 0.0 0.1 -0.0 + 10 3 2.0 2.4 0.1 -0.2 + 10 4 -1.0 5.3 -0.0 0.1 + 10 5 -0.6 -9.1 -0.3 -0.1 + 10 6 -0.9 0.4 0.0 0.1 + 10 7 1.5 -4.2 -0.1 0.0 + 10 8 0.9 -3.8 -0.1 -0.1 + 10 9 -2.7 0.9 -0.0 0.2 + 10 10 -3.9 -9.1 -0.0 -0.0 + 11 0 2.9 0.0 0.0 0.0 + 11 1 -1.5 0.0 -0.0 -0.0 + 11 2 -2.5 2.9 0.0 0.1 + 11 3 2.4 -0.6 0.0 -0.0 + 11 4 -0.6 0.2 0.0 0.1 + 11 5 -0.1 0.5 -0.1 -0.0 + 11 6 -0.6 -0.3 0.0 -0.0 + 11 7 -0.1 -1.2 -0.0 0.1 + 11 8 1.1 -1.7 -0.1 -0.0 + 11 9 -1.0 -2.9 -0.1 0.0 + 11 10 -0.2 -1.8 -0.1 0.0 + 11 11 2.6 -2.3 -0.1 0.0 + 12 0 -2.0 0.0 0.0 0.0 + 12 1 -0.2 -1.3 0.0 -0.0 + 12 2 0.3 0.7 -0.0 0.0 + 12 3 1.2 1.0 -0.0 -0.1 + 12 4 -1.3 -1.4 -0.0 0.1 + 12 5 0.6 -0.0 -0.0 -0.0 + 12 6 0.6 0.6 0.1 -0.0 + 12 7 0.5 -0.1 -0.0 -0.0 + 12 8 -0.1 0.8 0.0 0.0 + 12 9 -0.4 0.1 0.0 -0.0 + 12 10 -0.2 -1.0 -0.1 -0.0 + 12 11 -1.3 0.1 -0.0 0.0 + 12 12 -0.7 0.2 -0.1 -0.1 +999999999999999999999999999999999999999999999999 +999999999999999999999999999999999999999999999999 diff --git a/reference_standalone/original_project/reference/WMM2025.COF b/reference_standalone/original_project/reference/WMM2025.COF new file mode 100644 index 0000000..098c579 --- /dev/null +++ b/reference_standalone/original_project/reference/WMM2025.COF @@ -0,0 +1,93 @@ + 2025.0 WMM-2025 11/13/2024 + 1 0 -29351.8 0.0 12.0 0.0 + 1 1 -1410.8 4545.4 9.7 -21.5 + 2 0 -2556.6 0.0 -11.6 0.0 + 2 1 2951.1 -3133.6 -5.2 -27.7 + 2 2 1649.3 -815.1 -8.0 -12.1 + 3 0 1361.0 0.0 -1.3 0.0 + 3 1 -2404.1 -56.6 -4.2 4.0 + 3 2 1243.8 237.5 0.4 -0.3 + 3 3 453.6 -549.5 -15.6 -4.1 + 4 0 895.0 0.0 -1.6 0.0 + 4 1 799.5 278.6 -2.4 -1.1 + 4 2 55.7 -133.9 -6.0 4.1 + 4 3 -281.1 212.0 5.6 1.6 + 4 4 12.1 -375.6 -7.0 -4.4 + 5 0 -233.2 0.0 0.6 0.0 + 5 1 368.9 45.4 1.4 -0.5 + 5 2 187.2 220.2 0.0 2.2 + 5 3 -138.7 -122.9 0.6 0.4 + 5 4 -142.0 43.0 2.2 1.7 + 5 5 20.9 106.1 0.9 1.9 + 6 0 64.4 0.0 -0.2 0.0 + 6 1 63.8 -18.4 -0.4 0.3 + 6 2 76.9 16.8 0.9 -1.6 + 6 3 -115.7 48.8 1.2 -0.4 + 6 4 -40.9 -59.8 -0.9 0.9 + 6 5 14.9 10.9 0.3 0.7 + 6 6 -60.7 72.7 0.9 0.9 + 7 0 79.5 0.0 -0.0 0.0 + 7 1 -77.0 -48.9 -0.1 0.6 + 7 2 -8.8 -14.4 -0.1 0.5 + 7 3 59.3 -1.0 0.5 -0.8 + 7 4 15.8 23.4 -0.1 0.0 + 7 5 2.5 -7.4 -0.8 -1.0 + 7 6 -11.1 -25.1 -0.8 0.6 + 7 7 14.2 -2.3 0.8 -0.2 + 8 0 23.2 0.0 -0.1 0.0 + 8 1 10.8 7.1 0.2 -0.2 + 8 2 -17.5 -12.6 0.0 0.5 + 8 3 2.0 11.4 0.5 -0.4 + 8 4 -21.7 -9.7 -0.1 0.4 + 8 5 16.9 12.7 0.3 -0.5 + 8 6 15.0 0.7 0.2 -0.6 + 8 7 -16.8 -5.2 -0.0 0.3 + 8 8 0.9 3.9 0.2 0.2 + 9 0 4.6 0.0 -0.0 0.0 + 9 1 7.8 -24.8 -0.1 -0.3 + 9 2 3.0 12.2 0.1 0.3 + 9 3 -0.2 8.3 0.3 -0.3 + 9 4 -2.5 -3.3 -0.3 0.3 + 9 5 -13.1 -5.2 0.0 0.2 + 9 6 2.4 7.2 0.3 -0.1 + 9 7 8.6 -0.6 -0.1 -0.2 + 9 8 -8.7 0.8 0.1 0.4 + 9 9 -12.9 10.0 -0.1 0.1 + 10 0 -1.3 0.0 0.1 0.0 + 10 1 -6.4 3.3 0.0 0.0 + 10 2 0.2 0.0 0.1 -0.0 + 10 3 2.0 2.4 0.1 -0.2 + 10 4 -1.0 5.3 -0.0 0.1 + 10 5 -0.6 -9.1 -0.3 -0.1 + 10 6 -0.9 0.4 0.0 0.1 + 10 7 1.5 -4.2 -0.1 0.0 + 10 8 0.9 -3.8 -0.1 -0.1 + 10 9 -2.7 0.9 -0.0 0.2 + 10 10 -3.9 -9.1 -0.0 -0.0 + 11 0 2.9 0.0 0.0 0.0 + 11 1 -1.5 0.0 -0.0 -0.0 + 11 2 -2.5 2.9 0.0 0.1 + 11 3 2.4 -0.6 0.0 -0.0 + 11 4 -0.6 0.2 0.0 0.1 + 11 5 -0.1 0.5 -0.1 -0.0 + 11 6 -0.6 -0.3 0.0 -0.0 + 11 7 -0.1 -1.2 -0.0 0.1 + 11 8 1.1 -1.7 -0.1 -0.0 + 11 9 -1.0 -2.9 -0.1 0.0 + 11 10 -0.2 -1.8 -0.1 0.0 + 11 11 2.6 -2.3 -0.1 0.0 + 12 0 -2.0 0.0 0.0 0.0 + 12 1 -0.2 -1.3 0.0 -0.0 + 12 2 0.3 0.7 -0.0 0.0 + 12 3 1.2 1.0 -0.0 -0.1 + 12 4 -1.3 -1.4 -0.0 0.1 + 12 5 0.6 -0.0 -0.0 -0.0 + 12 6 0.6 0.6 0.1 -0.0 + 12 7 0.5 -0.1 -0.0 -0.0 + 12 8 -0.1 0.8 0.0 0.0 + 12 9 -0.4 0.1 0.0 -0.0 + 12 10 -0.2 -1.0 -0.1 -0.0 + 12 11 -1.3 0.1 -0.0 0.0 + 12 12 -0.7 0.2 -0.1 -0.1 +999999999999999999999999999999999999999999999999 +999999999999999999999999999999999999999999999999 diff --git a/reference_standalone/original_project/reference/WMM2025_TestValues.txt b/reference_standalone/original_project/reference/WMM2025_TestValues.txt new file mode 100644 index 0000000..64b2801 --- /dev/null +++ b/reference_standalone/original_project/reference/WMM2025_TestValues.txt @@ -0,0 +1,118 @@ +# Field 1: Decimal year +# Field 2: Altitude (km) +# Field 3: geodetic latitude (deg) +# Field 4: geodetic longitude (deg) +# Field 5: declination (deg) +# Field 6: inclination (deg) +# Field 7: H (nT) +# Field 8: X (nT) +# Field 9: Y (nT) +# Field 10: Z (nT) +# Field 11: F (nT) +# Field 12: dD/dt (deg/year) +# Field 13: dI/dt (deg/year) +# Field 14: dH/dt (nT/year) +# Field 15: dX/dt (nT/year) +# Field 16: dY/dt (nT/year) +# Field 17: dZ/dt (nT/year) +# Field 18: dF/dt (nT/year) +2025.000000 28 89 -121 -99.77 88.47 1504.298146 -255.388723 -1482.460628 56194.288771 56214.419888 2.491706 -0.009987 10.285640 62.723738 -21.242793 18.075146 18.343917 +2025.000000 48 80 -96 -29.91 87.77 2164.285547 1875.982280 -1079.269389 55623.044051 55665.134163 1.248417 -0.057537 55.469239 71.596398 13.214774 -12.148507 -9.982652 +2025.000000 54 82 87 54.89 87.68 2302.427342 1324.336929 1883.428620 56740.772059 56787.466800 0.884574 0.036130 -34.169939 -48.732002 -7.505574 41.134755 39.715524 +2025.000000 65 43 93 0.50 64.10 24300.764692 24299.852822 210.517066 50037.923998 55626.621348 -0.019813 0.030822 -3.938347 -3.865401 -8.437166 60.388819 52.601187 +2025.000000 51 -33 109 -5.49 -67.50 21838.046477 21737.778822 -2090.274098 -52710.003920 57054.752538 0.069861 0.026375 29.809763 32.221578 23.651711 -3.334040 14.490015 +2025.000000 39 -59 -8 -15.75 -58.55 14918.115796 14358.095523 -4049.107540 -24389.086374 28589.818346 0.010988 0.055916 -0.905656 -0.095123 2.999401 54.951779 -47.350226 +2025.000000 3 -50 -103 27.96 -54.89 22106.041373 19526.532799 10362.990980 -31437.562789 38431.724126 -0.032104 0.024888 -42.140973 -31.417096 -30.696074 88.952383 -97.003616 +2025.000000 94 -29 -110 15.74 -38.25 24181.990098 23275.471080 6559.046509 -19063.605287 30792.688931 -0.011062 0.027107 -42.453758 -39.595926 -16.008811 52.018933 -65.544285 +2025.000000 66 14 143 -0.19 12.82 35003.635649 35003.441364 -116.624818 7966.315182 35898.700342 -0.055086 -0.003859 10.848172 10.735986 -33.689325 -0.010939 10.575266 +2025.000000 18 0 21 1.29 -26.06 29282.246275 29274.811882 659.800118 -14316.722540 32594.761714 0.030544 0.079429 -8.761425 -9.110937 15.408838 54.581250 -31.844958 +2025.500000 6 -36 -137 20.28 -52.11 25353.230658 23781.930678 8786.698927 -32577.518648 41280.516301 0.025348 0.014224 -34.463126 -36.214518 -1.422651 60.968936 -69.281309 +2025.500000 63 26 81 0.51 41.07 34803.980117 34802.613433 308.431881 30332.056989 46166.554053 0.018525 0.021888 20.342054 20.241534 11.432574 41.122831 42.353703 +2025.500000 69 38 -144 12.93 56.97 23096.337032 22510.804524 5167.636206 35525.990264 42373.774537 -0.105300 -0.007297 -33.951051 -23.593092 -48.967345 -62.123761 -70.589724 +2025.500000 50 -70 -133 57.21 -71.94 16656.709403 9021.847823 14001.865232 -51084.838301 53731.803175 -0.036998 0.045634 8.590794 13.694592 1.395803 111.703074 -103.537175 +2025.500000 8 -52 -75 14.91 -49.63 20005.506364 19331.857134 5147.774727 -23532.664273 30886.996822 -0.098427 -0.028885 -58.502212 -47.688987 -48.263512 44.775999 -72.006513 +2025.500000 8 -66 17 -33.14 -59.55 18154.870136 15201.438652 -9925.501123 -30881.123197 35822.382382 -0.112991 0.041932 10.080437 -11.133189 -35.489343 34.582697 -24.703647 +2025.500000 22 -37 140 9.28 -68.62 21688.847590 21404.761773 3498.897430 -55397.587760 59492.006517 0.028818 0.004094 0.393984 -1.371020 10.829532 10.653875 -9.777010 +2025.500000 40 -12 -129 10.76 -15.46 29105.866326 28594.451238 5432.201489 -8052.525092 30199.248583 -0.009568 0.030404 -39.188467 -37.592705 -12.089297 27.469404 -45.094246 +2025.500000 44 33 -118 11.10 57.89 23678.743422 23235.835850 4558.379362 37727.715752 44542.826874 -0.075717 -0.021971 -41.700619 -34.896682 -38.734089 -98.573110 -105.659134 +2025.500000 50 -81 -67 28.13 -67.61 18292.842720 16132.682326 8623.494405 -44412.283868 48032.062762 -0.099291 0.019879 -12.853806 3.608124 -34.016532 74.964128 -74.210029 +2026.000000 74 -57 3 -22.51 -58.65 14362.206593 13268.119649 -5498.179626 -23576.062921 27606.226129 -0.037845 0.082339 12.945610 8.327751 -13.719801 55.005355 -40.240277 +2026.000000 46 -24 -122 14.01 -34.17 26638.500090 25846.118652 6448.863284 -18080.399376 32194.883578 0.000271 0.019932 -40.514451 -39.339842 -9.685735 41.034418 -56.566841 +2026.000000 69 23 63 1.17 35.92 34565.858527 34558.605297 708.078839 25043.405885 42684.549360 0.013234 0.010624 23.857027 23.688475 8.470762 27.058242 35.194683 +2026.000000 33 -3 -147 9.71 -2.12 30957.666082 30514.086409 5221.840663 -1146.978999 30978.906535 -0.006185 0.036855 -36.361404 -35.276692 -9.427361 21.287924 -37.124648 +2026.000000 47 -72 -22 -6.32 -61.16 18392.516222 18280.800055 -2024.105320 -33397.486160 38127.112857 -0.048225 0.017330 -15.939764 -17.546607 -13.632509 52.849287 -53.982732 +2026.000000 62 -14 99 -1.43 -44.70 33448.275663 33437.828630 -835.919473 -33100.922253 47058.030120 0.061400 0.072480 38.755947 39.639639 34.864408 45.397052 -4.385324 +2026.000000 83 86 -46 -30.61 86.84 3000.255301 2582.099403 -1527.839829 54279.308437 54362.163830 1.221388 -0.002547 3.280093 35.392266 53.372894 15.551936 15.709262 +2026.000000 82 -64 87 -81.74 -75.40 13974.248411 2007.074870 -13829.362571 -53663.514288 55453.154865 -0.187260 -0.029553 -23.144309 -48.522801 16.344602 -24.623137 17.996086 +2026.000000 34 -19 43 -14.98 -52.33 20212.448819 19525.694050 -5224.017527 -26182.862330 33076.961273 -0.135700 0.023383 51.024088 36.917850 -59.432251 -44.004821 66.012531 +2026.000000 56 -81 40 -59.77 -68.45 17877.818542 9000.536024 -15446.900890 -45267.890393 48670.301997 -0.146798 0.017031 -1.330470 -40.246526 -21.910829 42.754262 -40.254140 +2026.500000 14 0 80 -3.10 -17.15 39489.089663 39431.415992 -2133.456168 -12188.838466 41327.424134 0.056431 0.021103 33.618195 35.670346 37.019904 5.553327 30.484922 +2026.500000 12 -82 -68 29.79 -68.07 18497.480257 16052.819558 9190.416754 -45940.146795 49524.275496 -0.104864 0.021236 -11.118018 7.171828 -34.904116 76.756556 -75.354213 +2026.500000 44 -46 -42 -11.36 -54.39 14140.316272 13863.176808 -2785.834358 -19744.304022 24285.511845 0.039043 -0.119143 -63.277988 -60.139435 21.913412 1.623579 -38.163791 +2026.500000 43 17 52 1.19 23.95 36002.676553 35994.907968 747.876551 15990.895993 39394.180708 -0.010785 0.016571 20.351040 20.487426 -6.352799 21.505886 27.328663 +2026.500000 64 10 78 -1.53 7.53 39441.702532 39427.724480 -1049.971869 5214.811025 39784.948821 0.031231 0.021184 27.794943 28.357414 20.751458 18.512565 29.981674 +2026.500000 12 33 -145 11.96 52.51 24672.289649 24136.839639 5112.225422 32162.934508 40536.110231 -0.086620 0.000340 -40.973255 -32.355349 -44.980044 -53.017362 -67.004405 +2026.500000 12 -79 115 -137.58 -77.37 13023.480845 -9613.650418 -8785.714482 -58104.306533 59545.961164 -0.241722 0.014808 7.471685 -42.581077 35.518140 37.027107 -34.496496 +2026.500000 14 -33 -114 18.12 -44.10 24613.183584 23393.133584 7653.110952 -23854.293436 34275.882505 -0.000322 0.022971 -42.803882 -40.639067 -13.440897 60.620545 -72.925914 +2026.500000 19 29 66 2.24 46.04 32749.959268 32725.000691 1278.343380 33958.454002 47177.711160 0.018224 0.011101 23.712541 23.287876 11.334193 37.754815 43.636705 +2026.500000 86 -11 167 10.24 -31.60 33105.255278 32578.377044 5882.794932 -20368.224196 38869.300019 0.013426 -0.025948 -16.653141 -17.766581 4.674631 -10.422224 -8.722161 +2027.000000 37 -66 -5 -17.22 -59.04 17159.836500 16390.771268 -5079.626554 -28608.243575 33360.029814 -0.043630 0.039964 -2.074991 -5.850093 -11.867216 48.695530 -42.826703 +2027.000000 67 72 -115 13.73 84.84 5026.868699 4883.287833 1192.857435 55689.615237 55916.032175 -0.315901 -0.072636 66.581032 71.256140 -11.124661 -50.889259 -44.697541 +2027.000000 44 22 174 6.46 31.89 28867.487799 28683.972510 3249.857362 17961.198575 33999.066253 -0.024375 -0.000168 2.815221 4.179894 -11.885941 1.634357 3.253718 +2027.000000 54 54 178 0.63 65.46 20617.099795 20615.871559 225.041793 45149.631876 49634.202547 -0.170729 0.012944 -3.909066 -3.238256 -61.473597 18.434524 15.145168 +2027.000000 57 -43 50 -48.27 -63.13 16833.417225 11203.748204 -12563.437494 -33221.366617 37242.759503 -0.165742 -0.033376 12.323002 -28.141121 -41.606787 -72.317297 70.078525 +2027.000000 44 -43 -111 24.31 -52.57 22462.470699 20471.522067 9245.505620 -29347.556906 36957.295441 -0.006299 0.024728 -40.058633 -35.491603 -18.738696 78.579889 -86.747247 +2027.000000 12 -63 178 57.87 -79.14 11720.691727 6233.774935 9925.455386 -61075.730989 62190.188377 0.151793 0.036783 27.353301 -11.747306 39.678751 69.308326 -62.911163 +2027.000000 38 27 -169 8.48 42.66 26106.758229 25821.061601 3851.701313 24058.365347 35501.658671 -0.058765 0.013094 -19.135886 -14.976028 -29.306273 -6.601488 -18.545527 +2027.000000 61 59 -77 -16.48 78.68 10884.812802 10437.775689 -3087.391846 54397.713552 55476.034370 0.230418 -0.074136 59.669126 69.634646 25.051419 -67.643997 -54.621632 +2027.000000 67 -47 -32 -13.52 -57.98 12805.786103 12450.832543 -2994.148743 -20475.298079 24150.072239 0.115550 -0.077078 -52.470761 -44.977997 37.378183 22.627270 -47.007290 +2027.500000 8 62 53 19.39 76.67 12997.751893 12260.425578 4315.497527 54849.810218 56368.814385 0.099601 0.030088 -12.884268 -19.655269 17.035218 74.003425 69.038304 +2027.500000 77 -68 -7 -16.19 -59.82 17262.817301 16578.099495 -4813.676173 -29680.383821 34335.550744 -0.048399 0.032649 -5.108702 -8.972297 -12.579376 47.699056 -43.800575 +2027.500000 98 -5 159 7.79 -23.22 33857.496691 33544.960996 4589.735713 -14525.780052 36841.937629 -0.005671 -0.036102 -11.751062 -11.188278 -4.913401 -20.218524 -2.827531 +2027.500000 34 -29 -107 15.64 -37.45 24446.053109 23540.396213 6592.363667 -18723.097084 30792.269761 -0.017007 0.030517 -44.335923 -40.736596 -18.943541 54.615130 -68.406867 +2027.500000 60 27 65 1.85 42.83 33079.422542 33062.159421 1068.555172 30667.425573 45108.083389 0.016699 0.011093 23.388908 23.065275 10.391374 33.592654 39.990433 +2027.500000 73 -72 95 -102.64 -76.49 13306.747292 -2912.418045 -12984.118939 -55399.217725 56974.931751 -0.235287 -0.006801 -8.442550 -51.471801 20.197772 6.191978 -7.992526 +2027.500000 96 -46 -85 17.93 -47.37 19914.457641 18947.658009 6129.590452 -21631.434316 29402.458634 -0.091418 -0.005439 -53.229644 -40.865392 -46.615842 53.698344 -75.558704 +2027.500000 0 -13 -59 -17.49 -15.26 22401.493010 21365.849306 -6732.560620 -6112.313520 23220.406233 -0.173690 -0.385680 -68.873971 -86.099325 -44.070217 -143.226783 -28.743372 +2027.500000 16 66 -178 0.37 75.67 13821.614337 13821.326215 89.244328 54092.646409 55830.559897 -0.327448 0.003026 -1.280867 -0.770805 -78.997758 6.899408 6.367545 +2027.500000 72 -87 38 -65.44 -70.97 16661.696834 6926.051596 -15153.941754 -48295.635435 51088.947370 -0.148310 0.021340 0.540463 -39.001360 -18.419660 56.777921 -53.497300 +2028.000000 49 20 167 5.10 26.82 30251.262453 30131.436134 2689.876669 15295.611788 33898.298187 -0.020383 -0.011359 9.497816 10.417136 -9.874921 -2.728173 7.244961 +2028.000000 71 5 -13 -6.47 -17.66 28323.200567 28142.647915 -3192.970202 -9017.928693 29724.177504 0.166481 -0.073692 -18.125869 -8.732692 83.815865 -34.350117 -6.850172 +2028.000000 95 14 65 -0.51 17.44 36933.940251 36932.466767 -329.910612 11601.180703 38713.089985 0.011924 0.011881 29.013291 29.080794 7.427118 17.527263 32.932326 +2028.000000 86 -85 -79 41.09 -70.25 16867.459172 12713.115116 11085.480728 -46988.258092 49924.018042 -0.121705 0.024621 -3.420695 20.969078 -29.252744 73.025430 -69.886927 +2028.000000 30 -36 -64 -4.65 -40.08 17398.651081 17341.333537 -1411.102609 -14639.967305 22738.551012 -0.162162 -0.152944 -69.721104 -73.485198 -43.425730 -20.660569 -40.045784 +2028.000000 75 79 125 -18.59 87.42 2582.332595 2447.595900 -823.235047 57308.751756 57366.902212 -0.910648 0.022931 -21.221891 -33.198946 -32.136170 39.082660 38.087754 +2028.000000 21 6 -32 -14.34 -8.70 28453.070088 27567.094260 -7045.034525 -4352.046687 28783.980055 0.177960 -0.311470 -29.041854 -6.255704 92.814073 -153.852374 -5.445988 +2028.000000 1 -76 -75 29.87 -65.23 19597.635210 16993.742946 9761.147808 -42479.847250 46782.525885 -0.086171 0.016127 -22.854241 -5.137167 -36.941289 80.971555 -83.098300 +2028.000000 45 -46 -41 -11.68 -54.96 13897.562372 13609.973828 -2812.623735 -19816.196211 24203.798713 0.048411 -0.117246 -62.721602 -59.047199 24.193263 3.174357 -38.612991 +2028.000000 11 -22 -21 -23.24 -57.67 13528.270036 12430.606447 -5337.987778 -21373.837958 25295.356080 0.176027 -0.196560 -92.571749 -68.660974 74.716931 -16.002042 -35.987260 +2028.500000 28 54 -120 15.43 73.74 15286.094495 14735.145798 4066.959949 52393.678137 54578.037650 -0.147623 -0.053732 20.801434 30.530225 -32.430768 -111.448817 -101.162318 +2028.500000 68 -58 156 41.57 -81.52 9282.943032 6944.959164 6159.591995 -62264.415929 62952.605366 0.218259 0.014957 11.274695 -15.028950 33.936954 35.824068 -33.769886 +2028.500000 39 -65 -88 29.45 -60.20 20609.270068 17946.905687 10131.662696 -35982.872979 41466.964689 -0.076076 0.014457 -36.693634 -18.500934 -41.868201 85.117374 -92.097328 +2028.500000 27 -23 81 -13.27 -58.58 25625.329284 24940.672257 -5883.907569 -41948.733657 49156.421313 0.129297 -0.007968 18.629651 31.409843 52.004836 -43.610533 46.927695 +2028.500000 11 34 0 1.57 46.77 29089.194286 29078.234597 798.434048 30945.577813 42471.284539 0.111314 0.010464 15.380882 13.823891 56.915296 27.686933 30.707939 +2028.500000 72 -62 65 -67.87 -68.50 17434.847799 6567.891205 -16150.440331 -44267.327811 47576.992646 -0.176819 -0.022421 -7.894025 -52.815238 -12.956527 -30.762466 25.729685 +2028.500000 55 86 70 67.64 87.57 2370.361097 901.741754 2192.139033 55926.154052 55976.363930 1.341225 0.014915 -13.210618 -56.340956 8.891342 32.415740 31.827250 +2028.500000 59 32 163 0.15 43.10 28217.204381 28217.104175 75.200106 26405.862611 38645.571587 -0.043407 0.005164 13.000120 13.057045 -21.342658 16.936400 21.064439 +2028.500000 65 48 148 -9.55 61.79 23693.546804 23365.167010 -3931.047029 44177.553125 50130.233993 -0.040524 0.019409 4.495479 1.652822 -17.271572 44.310648 41.173752 +2028.500000 95 30 28 4.56 44.27 29786.406936 29692.133298 2367.965026 29039.808439 41599.765773 0.034245 0.035803 8.349023 6.907310 18.410171 44.444119 37.003480 +2029.000000 95 -60 -59 8.58 -55.17 18095.598879 17893.018948 2700.105868 -26011.845842 31687.013474 -0.053323 -0.035373 -49.689333 -46.620155 -24.066802 37.170306 -58.889314 +2029.000000 95 -70 42 -55.06 -64.54 18202.660480 10426.249753 -14920.796381 -38237.047006 42348.655378 -0.157751 0.012766 3.604264 -39.016467 -31.660684 14.381378 -11.435882 +2029.000000 50 87 -154 -73.48 89.07 906.920459 257.877420 -869.484879 55992.308183 55999.652503 1.562405 -0.064156 62.930173 41.603892 -53.300475 13.406039 14.423442 +2029.000000 58 32 19 4.11 46.03 29434.989012 29359.395781 2108.188210 30508.465542 42393.219362 0.056569 0.028397 10.593100 8.484435 29.745872 41.240139 37.033780 +2029.000000 57 34 -13 -1.89 45.74 28257.277809 28241.863242 -933.225495 28997.458189 40488.595068 0.154151 -0.026819 21.195169 23.694393 75.283010 -5.404499 10.921620 +2029.000000 38 -76 49 -64.28 -67.36 18412.640681 7991.121323 -16588.167977 -44151.303941 47836.837024 -0.160186 0.012467 -0.041101 -46.394534 -22.304306 27.140173 -25.065010 +2029.000000 49 -50 -179 32.11 -71.33 18080.593790 15315.042731 9610.272521 -53522.651338 56494.088877 0.110906 0.012602 -5.111774 -22.932228 26.927867 53.956270 -52.754307 +2029.000000 90 -55 -171 38.65 -72.79 16416.538468 12821.507915 10252.398258 -52995.143972 55479.618058 0.117199 0.024083 2.622098 -18.923406 27.863957 70.344114 -66.418097 +2029.000000 41 42 -19 -4.13 56.44 24503.475397 24439.977735 -1762.893879 36929.897501 44319.720622 0.180959 -0.034375 24.712482 30.216258 75.411786 -10.849192 4.622821 +2029.000000 19 46 -22 -5.65 60.89 22632.016516 22522.142232 -2227.393290 40651.688191 46527.066578 0.196585 -0.034725 25.397648 32.916674 74.775309 -12.351198 1.562595 +2029.500000 31 13 -132 9.04 31.41 28145.022897 27795.620707 4421.061344 17187.983590 32978.312476 -0.039071 0.025976 -57.604250 -53.874337 -28.002848 -17.659477 -58.365744 +2029.500000 93 -2 158 7.09 -17.84 34067.301020 33806.520595 4207.156292 -10964.647547 35788.329028 -0.010065 -0.038403 -8.149803 -7.348347 -6.945244 -22.576489 -0.841015 +2029.500000 51 -76 40 -56.34 -66.22 18517.441118 10263.650094 -15412.758102 -42018.541264 45917.898858 -0.148779 0.015688 0.140771 -39.943896 -26.768531 30.856345 -28.179249 +2029.500000 64 22 -132 10.23 43.76 26014.780783 25601.266218 4619.955329 24914.378368 36020.758857 -0.057237 0.010172 -55.619345 -50.120044 -35.452393 -44.412257 -70.887703 +2029.500000 26 -65 55 -63.48 -65.71 18695.741849 8347.198524 -16728.868464 -41431.528035 45454.397791 -0.179396 -0.005003 1.416835 -51.746426 -27.403316 -12.788863 12.239760 +2029.500000 66 -21 32 -14.63 -56.68 16100.322907 15578.335534 -4066.430831 -24494.877642 29312.444941 -0.198629 0.076846 37.723336 22.403071 -63.533684 14.184486 8.866905 +2029.500000 18 9 -172 9.24 15.85 30922.514410 30520.999126 4966.941695 8779.472279 32144.689001 -0.000053 0.016657 -17.137150 -16.910013 -2.781046 4.848633 -15.161302 +2029.500000 63 88 26 36.52 87.37 2539.900024 2041.140972 1511.567287 55286.620082 55344.931586 1.375737 0.002257 -1.029565 -37.121853 48.397411 25.084712 25.011033 +2029.500000 33 17 5 0.89 13.77 34026.126581 34021.976056 531.446418 8341.137769 35033.582023 0.078642 0.026998 3.099752 2.369935 46.745426 17.756374 7.238224 +2029.500000 77 -18 138 4.45 -47.55 31847.600506 31751.497580 2472.257962 -34817.395113 47186.021876 -0.036580 0.012491 0.895474 2.471158 -20.201885 14.262673 -9.919684 diff --git a/reference_standalone/original_project/src/control.cpp b/reference_standalone/original_project/src/control.cpp new file mode 100644 index 0000000..3776983 --- /dev/null +++ b/reference_standalone/original_project/src/control.cpp @@ -0,0 +1,47 @@ +#include "modules.hpp" +#include + +// This is intentionally a placeholder controller. +// The real magnetorquer team code should replace this file once their interface is ready. +// For now the controller takes the smoothed body-rate and magnetic-field estimate and returns +// a coil-current command in body axes. +void control_compute(const SimContext& ctx, Vec3& current_cmd_A) { + const Vec3 B_body_T = ctx.BfieldNav_T; + const Vec3 w_body_rad_s = ctx.pqrNav_rad_s; + + const double Bmag = norm(B_body_T); + if (Bmag < 1e-12) { + current_cmd_A = {0.0, 0.0, 0.0}; + return; + } + + // This is a simple rate-cross-field damping law. + // It is useful for closed-loop bring-up, but it is not the final flight controller. + const double dipoleCommandGain = 67200.0; + const Vec3 desiredDipole_body_Am2 = dipoleCommandGain * cross(w_body_rad_s, B_body_T); + + // Clamp each axis separately because each coil has its own current limit. + auto clamp = [](double v, double lo, double hi) { + return (v < lo) ? lo : ((v > hi) ? hi : v); + }; + + auto allowed_current = [](double dipoleGain_Am2_A, double currentLimit_A, double dipoleLimit_Am2) { + if (dipoleGain_Am2_A <= 0.0 || currentLimit_A <= 0.0 || dipoleLimit_Am2 <= 0.0) { + return 0.0; + } + const double currentLimitFromDipole_A = dipoleLimit_Am2 / dipoleGain_Am2_A; + return std::fmin(currentLimit_A, currentLimitFromDipole_A); + }; + + const double ixLimit_A = allowed_current(ctx.mtqDipoleGain_Am2_A.x, ctx.mtqCurrentLimit_A.x, ctx.mtqDipoleLimit_Am2.x); + const double iyLimit_A = allowed_current(ctx.mtqDipoleGain_Am2_A.y, ctx.mtqCurrentLimit_A.y, ctx.mtqDipoleLimit_Am2.y); + const double izLimit_A = allowed_current(ctx.mtqDipoleGain_Am2_A.z, ctx.mtqCurrentLimit_A.z, ctx.mtqDipoleLimit_Am2.z); + + const double rawIx_A = desiredDipole_body_Am2.x / ctx.mtqDipoleGain_Am2_A.x; + const double rawIy_A = desiredDipole_body_Am2.y / ctx.mtqDipoleGain_Am2_A.y; + const double rawIz_A = desiredDipole_body_Am2.z / ctx.mtqDipoleGain_Am2_A.z; + + current_cmd_A.x = clamp(rawIx_A, -ixLimit_A, ixLimit_A); + current_cmd_A.y = clamp(rawIy_A, -iyLimit_A, iyLimit_A); + current_cmd_A.z = clamp(rawIz_A, -izLimit_A, izLimit_A); +} diff --git a/reference_standalone/original_project/src/disturbance.cpp b/reference_standalone/original_project/src/disturbance.cpp new file mode 100644 index 0000000..22585d7 --- /dev/null +++ b/reference_standalone/original_project/src/disturbance.cpp @@ -0,0 +1,202 @@ +#include "modules.hpp" +#include + +namespace { + +struct AtmosphereLayer { + double baseAltitude_km; + double baseDensity_kg_m3; + double scaleHeight_km; +}; + +// Piecewise exponential atmosphere based on the standard engineering table that is +// commonly used for quick LEO density estimates out to 1000 km. +constexpr AtmosphereLayer kAtmosphereLayers[] = { + {0.0, 1.2250000000000000, 7.249}, + {25.0, 3.899e-2, 6.349}, + {30.0, 1.774e-2, 6.682}, + {40.0, 3.972e-3, 7.554}, + {50.0, 1.057e-3, 8.382}, + {60.0, 3.206e-4, 7.714}, + {70.0, 8.770e-5, 6.549}, + {80.0, 1.905e-5, 5.799}, + {90.0, 3.396e-6, 5.382}, + {100.0, 5.297e-7, 5.877}, + {110.0, 9.661e-8, 7.263}, + {120.0, 2.438e-8, 9.473}, + {130.0, 8.484e-9, 12.636}, + {140.0, 3.845e-9, 16.149}, + {150.0, 2.070e-9, 22.523}, + {180.0, 5.464e-10, 29.740}, + {200.0, 2.789e-10, 37.105}, + {250.0, 7.248e-11, 45.546}, + {300.0, 2.418e-11, 53.628}, + {350.0, 9.518e-12, 53.298}, + {400.0, 3.725e-12, 58.515}, + {450.0, 1.585e-12, 60.828}, + {500.0, 6.967e-13, 63.822}, + {600.0, 1.454e-13, 71.835}, + {700.0, 3.614e-14, 88.667}, + {800.0, 1.170e-14, 124.640}, + {900.0, 5.245e-15, 181.050}, + {1000.0, 3.019e-15, 268.000} +}; + +constexpr int kNumAtmosphereLayers = static_cast(sizeof(kAtmosphereLayers) / sizeof(kAtmosphereLayers[0])); + +double wrap_degrees_360(double angle_deg) { + double wrapped = std::fmod(angle_deg, 360.0); + if (wrapped < 0.0) { + wrapped += 360.0; + } + return wrapped; +} + +Vec3 earth_rotation_eci_rad_s(const SimContext& ctx) { + return {0.0, 0.0, ctx.earthRotationRate_rad_s}; +} + +double projected_box_area_m2(const Vec3& look_body_hat, const SimContext& ctx) { + const double areaYZ_m2 = ctx.ly_m * ctx.lz_m; + const double areaXZ_m2 = ctx.lx_m * ctx.lz_m; + const double areaXY_m2 = ctx.lx_m * ctx.ly_m; + + return std::abs(look_body_hat.x) * areaYZ_m2 + + std::abs(look_body_hat.y) * areaXZ_m2 + + std::abs(look_body_hat.z) * areaXY_m2; +} + +Vec3 sun_direction_eci(double t_s, const SimContext& ctx) { + const double daysSinceJ2000 = 365.25 * (ctx.decimalYear0 - 2000.0) + t_s / 86400.0; + const double meanLongitude_deg = wrap_degrees_360(280.460 + 0.9856474 * daysSinceJ2000); + const double meanAnomaly_deg = wrap_degrees_360(357.528 + 0.9856003 * daysSinceJ2000); + + const double meanLongitude_rad = meanLongitude_deg * M_PI / 180.0; + const double meanAnomaly_rad = meanAnomaly_deg * M_PI / 180.0; + const double eclipticLongitude_rad = + meanLongitude_rad + + (1.915 * M_PI / 180.0) * std::sin(meanAnomaly_rad) + + (0.020 * M_PI / 180.0) * std::sin(2.0 * meanAnomaly_rad); + const double obliquity_rad = (23.439 - 0.0000004 * daysSinceJ2000) * M_PI / 180.0; + + const Vec3 sunVecEci{ + std::cos(eclipticLongitude_rad), + std::cos(obliquity_rad) * std::sin(eclipticLongitude_rad), + std::sin(obliquity_rad) * std::sin(eclipticLongitude_rad) + }; + return normalize(sunVecEci); +} + +bool in_cylindrical_eclipse(const Vec3& pos_eci_m, const Vec3& sunHat_eci, const SimContext& ctx) { + const double alongSun_m = dot(pos_eci_m, sunHat_eci); + if (alongSun_m >= 0.0) { + return false; + } + + const Vec3 perpendicular_m = pos_eci_m - alongSun_m * sunHat_eci; + return norm(perpendicular_m) < ctx.earthRadius_m; +} + +} // namespace + +double density_kg_m3(double altitude_m) { + const double altitude_km = altitude_m / 1000.0; + + if (altitude_km <= kAtmosphereLayers[0].baseAltitude_km) { + return kAtmosphereLayers[0].baseDensity_kg_m3; + } + + for (int i = 0; i < kNumAtmosphereLayers - 1; ++i) { + const AtmosphereLayer& layer = kAtmosphereLayers[i]; + const AtmosphereLayer& nextLayer = kAtmosphereLayers[i + 1]; + if (altitude_km < nextLayer.baseAltitude_km) { + return layer.baseDensity_kg_m3 * + std::exp(-(altitude_km - layer.baseAltitude_km) / layer.scaleHeight_km); + } + } + + const AtmosphereLayer& topLayer = kAtmosphereLayers[kNumAtmosphereLayers - 1]; + return topLayer.baseDensity_kg_m3 * + std::exp(-(altitude_km - topLayer.baseAltitude_km) / topLayer.scaleHeight_km); +} + +void disturbance_summary(double t_s, + double altitude_m, + const Vec3& pos_eci_m, + const Vec3& vel_eci_m_s, + const Quat& q_ib, + const Vec3& B_body_T, + const SimContext& ctx, + DisturbanceBreakdown& summary) { + summary = DisturbanceBreakdown{}; + + summary.density_kg_m3 = density_kg_m3(altitude_m); + const Mat3 C_IB = TIBquat(q_ib); + + // The upper atmosphere corotates with the Earth to first order, so drag depends + // on velocity relative to that rotating frame rather than inertial speed. + const Vec3 atmosphereVel_eci_m_s = cross(earth_rotation_eci_rad_s(ctx), pos_eci_m); + const Vec3 relVel_eci_m_s = vel_eci_m_s - atmosphereVel_eci_m_s; + summary.relativeSpeed_m_s = norm(relVel_eci_m_s); + + if (summary.relativeSpeed_m_s > 1e-9 && summary.density_kg_m3 > 0.0) { + const Vec3 relVelHat_eci = relVel_eci_m_s / summary.relativeSpeed_m_s; + const Vec3 relVelHat_body = mul(transpose(C_IB), relVelHat_eci); + const double projectedArea_m2 = projected_box_area_m2(relVelHat_body, ctx); + + const double dragMag_N = + 0.5 * summary.density_kg_m3 * + summary.relativeSpeed_m_s * + summary.relativeSpeed_m_s * + ctx.dragCoeff * + projectedArea_m2; + + summary.dragForce_eci_N = -dragMag_N * relVelHat_eci; + const Vec3 dragForce_body_N = mul(transpose(C_IB), summary.dragForce_eci_N); + summary.aerodynamicTorque_body_Nm = cross(ctx.aeroCpOffset_body_m, dragForce_body_N); + } + + const double radius_m = norm(pos_eci_m); + if (radius_m > 1e-9) { + const Vec3 rhat_eci = pos_eci_m / radius_m; + const Vec3 rhat_body = mul(transpose(C_IB), rhat_eci); + const Vec3 Irhat_body = mul(ctx.inertia_body_kgm2, rhat_body); + + const double ggGain = 3.0 * ctx.mu_m3_s2 / (radius_m * radius_m * radius_m); + summary.gravityGradientTorque_body_Nm = ggGain * cross(rhat_body, Irhat_body); + } + + // First-pass residual magnetic disturbance from public magnetorquer remanence + // data: CR0002 rods are bounded at <0.1% FSR and the EXA MT01 sheet lists + // <0.0045 A m^2 residual moment. + summary.residualMagneticTorque_body_Nm = cross(ctx.residualDipole_body_Am2, B_body_T); + + const Vec3 sunHat_eci = sun_direction_eci(t_s, ctx); + summary.sunlit = !in_cylindrical_eclipse(pos_eci_m, sunHat_eci, ctx); + if (summary.sunlit) { + const Vec3 sunHat_body = mul(transpose(C_IB), sunHat_eci); + const double projectedArea_m2 = projected_box_area_m2(sunHat_body, ctx); + const double srpForceMag_N = + ctx.solarRadiationPressure_N_m2 * ctx.solarReflectivityCoeff * projectedArea_m2; + + // Force points away from the Sun because photons transfer momentum to the spacecraft. + summary.solarRadiationForce_eci_N = srpForceMag_N * sunHat_eci; + const Vec3 srpForce_body_N = mul(transpose(C_IB), summary.solarRadiationForce_eci_N); + summary.solarRadiationTorque_body_Nm = cross(ctx.srpCpOffset_body_m, srpForce_body_N); + } +} + +void disturbance(double t_s, + double altitude_m, + const Vec3& pos_eci_m, + const Vec3& vel_eci_m_s, + const Quat& q_ib, + const Vec3& B_body_T, + const SimContext& ctx, + Vec3& disturbanceForce_eci_N, + Vec3& disturbanceTorque_body_Nm) { + DisturbanceBreakdown summary{}; + disturbance_summary(t_s, altitude_m, pos_eci_m, vel_eci_m_s, q_ib, B_body_T, ctx, summary); + disturbanceForce_eci_N = summary.totalForce_eci_N(); + disturbanceTorque_body_Nm = summary.totalTorque_body_Nm(); +} diff --git a/reference_standalone/original_project/src/main.cpp b/reference_standalone/original_project/src/main.cpp new file mode 100644 index 0000000..40ffbe8 --- /dev/null +++ b/reference_standalone/original_project/src/main.cpp @@ -0,0 +1,186 @@ +#include "modules.hpp" +#include "params.hpp" +#include +#include +#include +#include + +namespace { + +State13 add(const State13& a, const State13& b) { + State13 c{}; + for (int i = 0; i < 13; ++i) { + c[i] = a[i] + b[i]; + } + return c; +} + +State13 scale(const State13& a, double s) { + State13 c{}; + for (int i = 0; i < 13; ++i) { + c[i] = a[i] * s; + } + return c; +} + +bool all_finite(const State13& a) { + for (double v : a) { + if (!std::isfinite(v)) { + return false; + } + } + return true; +} + +} // namespace + +int main() { + // Fixed seed makes the sensor noise repeatable while debugging. + SimContext ctx(/*seed=*/1); + initialize_simulation(ctx); + + // Start in a 600 km circular orbit. + const double altitude_m = 600.0 * 1000.0; + const double x0 = ctx.earthRadius_m + altitude_m; + const double y0 = 0.0; + const double z0 = 0.0; + const double xdot0 = 0.0; + + const double inclination_rad = 56.0 * M_PI / 180.0; + const double semiMajor_m = std::sqrt(x0 * x0 + y0 * y0 + z0 * z0); + const double circularSpeed_m_s = std::sqrt(ctx.mu_m3_s2 / semiMajor_m); + const double ydot0 = circularSpeed_m_s * std::cos(inclination_rad); + const double zdot0 = circularSpeed_m_s * std::sin(inclination_rad); + + // Start with level attitude and a noticeable tumble rate. + const Quat q0 = euler321_to_quat({0.0, 0.0, 0.0}); + const double p0 = 0.8; + const double qRate0 = -0.2; + const double r0 = 0.3; + + State13 state{}; + state[0] = x0; state[1] = y0; state[2] = z0; + state[3] = xdot0; state[4] = ydot0; state[5] = zdot0; + state[6] = q0.q0; state[7] = q0.q1; state[8] = q0.q2; state[9] = q0.q3; + state[10] = p0; state[11] = qRate0; state[12] = r0; + + // Run for one orbit. + const double orbitalPeriod_s = 2.0 * M_PI / std::sqrt(ctx.mu_m3_s2) * std::pow(semiMajor_m, 1.5); + const double tfinal_s = orbitalPeriod_s; + const double dt_s = SimParams::timestep; + + // Discrete update clocks. + double nextSensorSample_s = 0.0; + double nextControlUpdate_s = 0.0; + const double controlPeriod_s = 0.1; + + std::ofstream csv("adcs_output.csv"); + csv << std::setprecision(17); + csv << "t," + << "x,y,z,xdot,ydot,zdot,q0,q1,q2,q3,p,q,r," + << "BBx,BBy,BBz,Bmx,Bmy,Bmz,BNx,BNy,BNz," + << "pqrm_x,pqrm_y,pqrm_z,pqrN_x,pqrN_y,pqrN_z," + << "ptpN_phi,ptpN_theta,ptpN_psi,qN0,qN1,qN2,qN3," + << "ix,iy,iz," + << "mcmd_x,mcmd_y,mcmd_z," + << "pcoil_x,pcoil_y,pcoil_z,pcoil_total," + << "rho_kg_m3,vrel_m_s,sunlit," + << "Fdrag_x,Fdrag_y,Fdrag_z,Fsrp_x,Fsrp_y,Fsrp_z," + << "Taero_x,Taero_y,Taero_z,Tgg_x,Tgg_y,Tgg_z," + << "Tmag_x,Tmag_y,Tmag_z,Tsrp_x,Tsrp_y,Tsrp_z," + << "Tdist_x,Tdist_y,Tdist_z\n"; + + for (double t_s = 0.0; t_s <= tfinal_s + 1e-12; t_s += dt_s) { + // ----- Truth environment ----- + ctx.BfieldReference_eci_T = truth_magnetic_field_eci_T(t_s, state, ctx); + const Mat3 C_IB = TIBquat(state_quaternion(state)); + ctx.BfieldTruth_body_T = mul(transpose(C_IB), ctx.BfieldReference_eci_T); + + // ----- Sensor and nav updates ----- + if (t_s + 1e-12 >= nextSensorSample_s) { + sensor_update_from_truth(state, ctx.BfieldTruth_body_T, ctx); + navigation_update(ctx); + nextSensorSample_s += ctx.sensorPeriod_s; + } + + // ----- Controller update ----- + if (t_s + 1e-12 >= nextControlUpdate_s) { + control_compute(ctx, ctx.commandedCurrent_A); + nextControlUpdate_s += controlPeriod_s; + } + + // ----- Log current state ----- + const Vec3 dipole_body_Am2 = magnetorquer_dipole_body_Am2(ctx); + const Vec3 pcoil_W = magnetorquer_coil_power_W(ctx); + const double pcoilTotal_W = magnetorquer_total_coil_power_W(ctx); + DisturbanceBreakdown disturbanceNow{}; + const Vec3 pos_eci_m{state[0], state[1], state[2]}; + const Vec3 vel_eci_m_s{state[3], state[4], state[5]}; + const Quat qNow = state_quaternion(state); + const double radius_m = norm(pos_eci_m); + const double altitude_m = radius_m - ctx.earthRadius_m; + disturbance_summary(t_s, altitude_m, pos_eci_m, vel_eci_m_s, qNow, ctx.BfieldTruth_body_T, ctx, disturbanceNow); + const Vec3 totalDisturbanceTorque_body_Nm = disturbanceNow.totalTorque_body_Nm(); + + csv << t_s << ","; + for (int i = 0; i < 13; ++i) { + csv << state[i] << ","; + } + + csv << ctx.BfieldTruth_body_T.x << "," << ctx.BfieldTruth_body_T.y << "," << ctx.BfieldTruth_body_T.z << "," + << ctx.BfieldMeasured_body_T.x << "," << ctx.BfieldMeasured_body_T.y << "," << ctx.BfieldMeasured_body_T.z << "," + << ctx.BfieldNav_T.x << "," << ctx.BfieldNav_T.y << "," << ctx.BfieldNav_T.z << "," + << ctx.pqrMeasured_rad_s.x << "," << ctx.pqrMeasured_rad_s.y << "," << ctx.pqrMeasured_rad_s.z << "," + << ctx.pqrNav_rad_s.x << "," << ctx.pqrNav_rad_s.y << "," << ctx.pqrNav_rad_s.z << "," + << ctx.ptpNav_rad.x << "," << ctx.ptpNav_rad.y << "," << ctx.ptpNav_rad.z << "," + << ctx.qNav_IB.q0 << "," << ctx.qNav_IB.q1 << "," << ctx.qNav_IB.q2 << "," << ctx.qNav_IB.q3 << "," + << ctx.commandedCurrent_A.x << "," << ctx.commandedCurrent_A.y << "," << ctx.commandedCurrent_A.z << "," + << dipole_body_Am2.x << "," << dipole_body_Am2.y << "," << dipole_body_Am2.z << "," + << pcoil_W.x << "," << pcoil_W.y << "," << pcoil_W.z << "," << pcoilTotal_W << "," + << disturbanceNow.density_kg_m3 << "," << disturbanceNow.relativeSpeed_m_s << "," << disturbanceNow.sunlit << "," + << disturbanceNow.dragForce_eci_N.x << "," << disturbanceNow.dragForce_eci_N.y << "," << disturbanceNow.dragForce_eci_N.z << "," + << disturbanceNow.solarRadiationForce_eci_N.x << "," << disturbanceNow.solarRadiationForce_eci_N.y << "," << disturbanceNow.solarRadiationForce_eci_N.z << "," + << disturbanceNow.aerodynamicTorque_body_Nm.x << "," << disturbanceNow.aerodynamicTorque_body_Nm.y << "," << disturbanceNow.aerodynamicTorque_body_Nm.z << "," + << disturbanceNow.gravityGradientTorque_body_Nm.x << "," << disturbanceNow.gravityGradientTorque_body_Nm.y << "," << disturbanceNow.gravityGradientTorque_body_Nm.z << "," + << disturbanceNow.residualMagneticTorque_body_Nm.x << "," << disturbanceNow.residualMagneticTorque_body_Nm.y << "," << disturbanceNow.residualMagneticTorque_body_Nm.z << "," + << disturbanceNow.solarRadiationTorque_body_Nm.x << "," << disturbanceNow.solarRadiationTorque_body_Nm.y << "," << disturbanceNow.solarRadiationTorque_body_Nm.z << "," + << totalDisturbanceTorque_body_Nm.x << "," << totalDisturbanceTorque_body_Nm.y << "," << totalDisturbanceTorque_body_Nm.z + << "\n"; + + // ----- Integrate one RK4 step using the held actuator command ----- + const State13 k1 = satellite_derivatives(t_s, state, ctx); + const State13 k2 = satellite_derivatives(t_s + dt_s / 2.0, add(state, scale(k1, dt_s / 2.0)), ctx); + const State13 k3 = satellite_derivatives(t_s + dt_s / 2.0, add(state, scale(k2, dt_s / 2.0)), ctx); + const State13 k4 = satellite_derivatives(t_s + dt_s, add(state, scale(k3, dt_s)), ctx); + + if (!all_finite(k1) || !all_finite(k2) || !all_finite(k3) || !all_finite(k4)) { + std::cerr << "Non-finite RK4 derivative at t = " << t_s << " s\n"; + return 1; + } + + State13 incr{}; + for (int i = 0; i < 13; ++i) { + incr[i] = (1.0 / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]); + } + + for (int i = 0; i < 13; ++i) { + state[i] += dt_s * incr[i]; + } + + if (!all_finite(state)) { + std::cerr << "Non-finite propagated state at t = " << t_s << " s\n"; + return 1; + } + + // Keep the quaternion normalized so numerical drift does not slowly break attitude math. + Quat q = state_quaternion(state); + q = normalize(q); + state[6] = q.q0; + state[7] = q.q1; + state[8] = q.q2; + state[9] = q.q3; + } + + std::cout << "Done. Wrote adcs_output.csv\n"; + return 0; +} diff --git a/reference_standalone/original_project/src/navigation.cpp b/reference_standalone/original_project/src/navigation.cpp new file mode 100644 index 0000000..b3dad88 --- /dev/null +++ b/reference_standalone/original_project/src/navigation.cpp @@ -0,0 +1,90 @@ +#include "modules.hpp" + +namespace { + +Quat integrate_quat_forward_euler(const Quat& q, const Vec3& omega_body_rad_s, double dt_s) { + const Quat qdot = quat_derivative(q, omega_body_rad_s); + return normalize({ + q.q0 + dt_s * qdot.q0, + q.q1 + dt_s * qdot.q1, + q.q2 + dt_s * qdot.q2, + q.q3 + dt_s * qdot.q3 + }); +} + +Quat quat_from_two_unit_vectors(const Vec3& from_unit, const Vec3& to_unit) { + const double dotv = dot(from_unit, to_unit); + const Vec3 axis = cross(from_unit, to_unit); + const double w = 1.0 + dotv; + + if (w < 1e-9) { + const Vec3 helper = (std::abs(from_unit.x) < 0.9) ? Vec3{1.0, 0.0, 0.0} : Vec3{0.0, 1.0, 0.0}; + const Vec3 ortho = normalize(cross(from_unit, helper)); + return {0.0, ortho.x, ortho.y, ortho.z}; + } + + return normalize({w, axis.x, axis.y, axis.z}); +} + +Vec3 predicted_body_field_T(const Quat& q_ib, const Vec3& referenceField_eci_T) { + return mul(transpose(TIBquat(q_ib)), referenceField_eci_T); +} + +} // namespace + +// This is a first realistic estimator scaffold. +// It propagates attitude with measured body rates and uses the expected magnetic +// field direction as a lightweight correction source. +void navigation_update(SimContext& ctx) { + const Vec3 Bmeas_body_T = ctx.BfieldMeasured_body_T; + const Vec3 Bref_eci_T = ctx.BfieldReference_eci_T; + const double BmeasNorm = norm(Bmeas_body_T); + const double BrefNorm = norm(Bref_eci_T); + const bool magneticReferenceValid = (BmeasNorm > 1e-12) && (BrefNorm > 1e-12); + + if (!ctx.navInitialized) { + if (magneticReferenceValid) { + ctx.qNav_IB = quat_from_two_unit_vectors(Bmeas_body_T / BmeasNorm, Bref_eci_T / BrefNorm); + } else { + ctx.qNav_IB = {1.0, 0.0, 0.0, 0.0}; + } + + ctx.BfieldNav_T = Bmeas_body_T; + ctx.pqrNav_rad_s = ctx.pqrMeasured_rad_s; + ctx.ptpNav_rad = quat_to_euler321(ctx.qNav_IB); + + ctx.BfieldNavPrev_T = ctx.BfieldNav_T; + ctx.pqrNavPrev_rad_s = ctx.pqrNav_rad_s; + ctx.ptpNavPrev_rad = ctx.ptpNav_rad; + ctx.BdotNav_T_s = {0.0, 0.0, 0.0}; + ctx.navInitialized = true; + return; + } + + const double dt_s = ctx.sensorPeriod_s; + const double s = ctx.navBlend; + const Vec3 gyroUnbiased_rad_s = ctx.pqrMeasured_rad_s - ctx.gyroBiasNav_rad_s; + + Vec3 correction_body_rad_s{0.0, 0.0, 0.0}; + if (magneticReferenceValid) { + const Vec3 Bpred_body_T = predicted_body_field_T(ctx.qNav_IB, Bref_eci_T); + correction_body_rad_s = cross(normalize(Bmeas_body_T), normalize(Bpred_body_T)); + } + + const Vec3 omegaCorrected_rad_s = gyroUnbiased_rad_s + ctx.navAttitudeCorrectionGain * correction_body_rad_s; + ctx.qNav_IB = integrate_quat_forward_euler(ctx.qNav_IB, omegaCorrected_rad_s, dt_s); + + if (magneticReferenceValid) { + ctx.gyroBiasNav_rad_s = ctx.gyroBiasNav_rad_s - (ctx.navBiasCorrectionGain * dt_s) * correction_body_rad_s; + } + + ctx.BfieldNav_T = (1.0 - s) * ctx.BfieldNavPrev_T + s * Bmeas_body_T; + ctx.pqrNav_rad_s = omegaCorrected_rad_s; + ctx.ptpNav_rad = quat_to_euler321(ctx.qNav_IB); + + ctx.BdotNav_T_s = (ctx.BfieldNav_T - ctx.BfieldNavPrev_T) / ctx.sensorPeriod_s; + + ctx.BfieldNavPrev_T = ctx.BfieldNav_T; + ctx.pqrNavPrev_rad_s = ctx.pqrNav_rad_s; + ctx.ptpNavPrev_rad = ctx.ptpNav_rad; +} diff --git a/reference_standalone/original_project/src/satellite.cpp b/reference_standalone/original_project/src/satellite.cpp new file mode 100644 index 0000000..8574710 --- /dev/null +++ b/reference_standalone/original_project/src/satellite.cpp @@ -0,0 +1,220 @@ +#include "modules.hpp" +#include "frames.hpp" +#include "quaternion.hpp" +#include "wmm.hpp" +#include + +namespace { + +Vec3 state_position_eci_m(const State13& state) { + return {state[0], state[1], state[2]}; +} + +Vec3 state_velocity_eci_m_s(const State13& state) { + return {state[3], state[4], state[5]}; +} + +double clamp_abs(double value, double limit) { + if (limit <= 0.0) { + return 0.0; + } + if (value > limit) { + return limit; + } + if (value < -limit) { + return -limit; + } + return value; +} + +} // namespace + +void initialize_simulation(SimContext& ctx) { + // Earth constants. + ctx.earthRadius_m = 6.371e6; + ctx.earthMass_kg = 5.972e24; + ctx.gravConst_SI = 6.67e-11; + ctx.mu_m3_s2 = ctx.gravConst_SI * ctx.earthMass_kg; + + // Spacecraft box geometry. + ctx.mass_kg = 2.6; + ctx.lx_m = 0.10; + ctx.ly_m = 0.10; + ctx.lz_m = 0.20; + ctx.maxArea_m2 = ctx.lx_m * ctx.ly_m; + ctx.maxMomentArm_m = ctx.lz_m / 2.0; + ctx.dragCoeff = 2.2; + ctx.solarRadiationPressure_N_m2 = 4.56e-6; + ctx.solarReflectivityCoeff = 1.3; + ctx.residualDipole_body_Am2 = {2.0e-4, 2.0e-4, 4.5e-3}; + ctx.aeroCpOffset_body_m = {0.0, 0.0, 0.0}; + ctx.srpCpOffset_body_m = {0.0, 0.0, 0.0}; + + // Box inertia in the body frame. + Mat3 I{}; + I[0][0] = (ctx.mass_kg / 12.0) * (ctx.ly_m * ctx.ly_m + ctx.lz_m * ctx.lz_m); + I[1][1] = (ctx.mass_kg / 12.0) * (ctx.lx_m * ctx.lx_m + ctx.lz_m * ctx.lz_m); + I[2][2] = (ctx.mass_kg / 12.0) * (ctx.lx_m * ctx.lx_m + ctx.ly_m * ctx.ly_m); + ctx.inertia_body_kgm2 = I; + ctx.inertiaInv_body_kgm2 = inv3(I); + + // HuskySat-2 first-pass actuator realism assumptions: + // - X/Y axes use CubeSpace CR0002 public datasheet values. + // - Z axis uses the EXA MT01 public marketplace sheet and keeps only the + // quantities we can defend today without driver-specific test data. + ctx.mtqDipoleGain_Am2_A = {2.3, 2.3, 0.85 / std::sqrt(1.75 / 4.4)}; + ctx.mtqResistance_Ohm = {51.0, 51.0, 4.4}; + ctx.mtqCurrentLimit_A = {5.0 / 51.0, 5.0 / 51.0, std::sqrt(1.75 / 4.4)}; + ctx.mtqDipoleLimit_Am2 = {0.2, 0.2, 0.85}; + ctx.commandedCurrent_A = {0.0, 0.0, 0.0}; + + // Sensor/nav defaults. + ctx.sensorPeriod_s = 1.0; + ctx.sensorModelInitialized = false; + ctx.navInitialized = false; + ctx.navBlend = 0.3; + ctx.navAttitudeCorrectionGain = 0.8; + ctx.navBiasCorrectionGain = 0.02; + + ctx.BfieldReference_eci_T = {0.0, 0.0, 0.0}; + ctx.BfieldTruth_body_T = {0.0, 0.0, 0.0}; + ctx.BfieldMeasured_body_T = {0.0, 0.0, 0.0}; + ctx.pqrMeasured_rad_s = {0.0, 0.0, 0.0}; + ctx.qNav_IB = {1.0, 0.0, 0.0, 0.0}; + ctx.gyroBiasNav_rad_s = {0.0, 0.0, 0.0}; +} + +Quat state_quaternion(const State13& state) { + return {state[6], state[7], state[8], state[9]}; +} + +Vec3 state_body_rates_rad_s(const State13& state) { + return {state[10], state[11], state[12]}; +} + +Vec3 state_euler321_rad(const State13& state) { + return quat_to_euler321(state_quaternion(state)); +} + +Vec3 truth_magnetic_field_eci_T(double t_s, const State13& state, const SimContext& ctx) { + const Vec3 r_eci_m = state_position_eci_m(state); + + // 1) Move the orbit position from inertial to Earth-fixed coordinates. + const double earthAngle = ctx.greenwichAngle0_rad + ctx.earthRotationRate_rad_s * t_s; + const Vec3 r_ecef_m = eci_to_ecef(r_eci_m, earthAngle); + + // 2) Convert Earth-fixed position to geodetic latitude, longitude, and altitude. + const GeodeticLLA lla = ecef_to_geodetic_wgs84(r_ecef_m); + + // 3) Ask WMM for the local North/East/Down field in nT. + const double decimalYear = ctx.decimalYear0 + t_s / (365.25 * 86400.0); + double X_nT = 0.0; + double Y_nT = 0.0; + double Z_nT = 0.0; + wmm2025_geodetic_ned_nT(lla.lat_rad * 180.0 / M_PI, + lla.lon_rad * 180.0 / M_PI, + lla.alt_m / 1000.0, + decimalYear, + X_nT, + Y_nT, + Z_nT); + + // 4) Convert the local NED vector to ECEF, then to ECI, then to body. + const Mat3 C_ecef_ned = ned_basis_ecef(lla.lat_rad, lla.lon_rad); + const Vec3 B_ned_nT{X_nT, Y_nT, Z_nT}; + const Vec3 B_ecef_nT = mul(C_ecef_ned, B_ned_nT); + const Vec3 B_eci_nT = ecef_to_eci(B_ecef_nT, earthAngle); + + // 5) Convert nT to Tesla before using the field in any torque calculation. + return 1e-9 * B_eci_nT; +} + +Vec3 truth_magnetic_field_body_T(double t_s, const State13& state, const SimContext& ctx) { + const Quat q = state_quaternion(state); + const Vec3 B_eci_T = truth_magnetic_field_eci_T(t_s, state, ctx); + + const Mat3 C_IB = TIBquat(q); + return mul(transpose(C_IB), B_eci_T); +} + +Vec3 magnetorquer_dipole_body_Am2(const SimContext& ctx) { + Vec3 dipole{ + ctx.mtqDipoleGain_Am2_A.x * ctx.commandedCurrent_A.x, + ctx.mtqDipoleGain_Am2_A.y * ctx.commandedCurrent_A.y, + ctx.mtqDipoleGain_Am2_A.z * ctx.commandedCurrent_A.z + }; + + dipole.x = clamp_abs(dipole.x, ctx.mtqDipoleLimit_Am2.x); + dipole.y = clamp_abs(dipole.y, ctx.mtqDipoleLimit_Am2.y); + dipole.z = clamp_abs(dipole.z, ctx.mtqDipoleLimit_Am2.z); + return dipole; +} + +Vec3 magnetorquer_coil_power_W(const SimContext& ctx) { + // Until we add explicit RL driver dynamics, the held command is the coil current. + return { + ctx.mtqResistance_Ohm.x * ctx.commandedCurrent_A.x * ctx.commandedCurrent_A.x, + ctx.mtqResistance_Ohm.y * ctx.commandedCurrent_A.y * ctx.commandedCurrent_A.y, + ctx.mtqResistance_Ohm.z * ctx.commandedCurrent_A.z * ctx.commandedCurrent_A.z + }; +} + +double magnetorquer_total_coil_power_W(const SimContext& ctx) { + const Vec3 pcoil_W = magnetorquer_coil_power_W(ctx); + return pcoil_W.x + pcoil_W.y + pcoil_W.z; +} + +State13 satellite_derivatives(double t_s, const State13& state, const SimContext& ctx) { + const Vec3 r_eci_m = state_position_eci_m(state); + const Vec3 v_eci_m_s = state_velocity_eci_m_s(state); + const Quat q = state_quaternion(state); + const Vec3 w_body_rad_s = state_body_rates_rad_s(state); + + const double radius_m = norm(r_eci_m); + const Vec3 rhat_eci = (radius_m > 0.0) ? (r_eci_m / radius_m) : Vec3{0.0, 0.0, 0.0}; + const double altitude_m = radius_m - ctx.earthRadius_m; + + // Central gravity in the inertial frame. + const Vec3 gravityForce_eci_N = (-(ctx.mu_m3_s2 * ctx.mass_kg) / (radius_m * radius_m)) * rhat_eci; + + // Compute the truth field directly from the current truth state. + const Vec3 B_body_T = truth_magnetic_field_body_T(t_s, state, ctx); + + // Simple disturbance model. + Vec3 disturbanceForce_eci_N{0.0, 0.0, 0.0}; + Vec3 disturbanceTorque_body_Nm{0.0, 0.0, 0.0}; + disturbance(t_s, altitude_m, r_eci_m, v_eci_m_s, q, B_body_T, ctx, disturbanceForce_eci_N, disturbanceTorque_body_Nm); + + // Magnetorquer torque in the body frame. + const Vec3 dipole_body_Am2 = magnetorquer_dipole_body_Am2(ctx); + const Vec3 controlTorque_body_Nm = cross(dipole_body_Am2, B_body_T); + + // Translational dynamics. + const Vec3 accel_eci_m_s2 = (gravityForce_eci_N + disturbanceForce_eci_N) / ctx.mass_kg; + + // Rotational dynamics. + const Quat qdot = quat_derivative(q, w_body_rad_s); + const Vec3 H_body = mul(ctx.inertia_body_kgm2, w_body_rad_s); + const Vec3 totalTorque_body_Nm = controlTorque_body_Nm + disturbanceTorque_body_Nm; + const Vec3 wdot_body_rad_s2 = mul(ctx.inertiaInv_body_kgm2, totalTorque_body_Nm - cross(w_body_rad_s, H_body)); + + State13 dst{}; + dst[0] = v_eci_m_s.x; + dst[1] = v_eci_m_s.y; + dst[2] = v_eci_m_s.z; + + dst[3] = accel_eci_m_s2.x; + dst[4] = accel_eci_m_s2.y; + dst[5] = accel_eci_m_s2.z; + + dst[6] = qdot.q0; + dst[7] = qdot.q1; + dst[8] = qdot.q2; + dst[9] = qdot.q3; + + dst[10] = wdot_body_rad_s2.x; + dst[11] = wdot_body_rad_s2.y; + dst[12] = wdot_body_rad_s2.z; + + return dst; +} diff --git a/reference_standalone/original_project/src/sensor.cpp b/reference_standalone/original_project/src/sensor.cpp new file mode 100644 index 0000000..086585c --- /dev/null +++ b/reference_standalone/original_project/src/sensor.cpp @@ -0,0 +1,36 @@ +#include "modules.hpp" + +namespace { + +Vec3 draw_uniform_noise(Rng& rng, double scale) { + return { + scale * rng.rand_m11(), + scale * rng.rand_m11(), + scale * rng.rand_m11() + }; +} + +void initialize_sensor_model(SimContext& ctx) { + // Fixed per-axis biases. These stay constant for the whole run. + ctx.magBias_T = draw_uniform_noise(ctx.rng, 4e-7); + ctx.gyroBias_rad_s = draw_uniform_noise(ctx.rng, 0.01); + + ctx.sensorModelInitialized = true; +} + +} // namespace + +void sensor_update_from_truth(const State13& truth_state, const Vec3& Btruth_body_T, SimContext& ctx) { + if (!ctx.sensorModelInitialized) { + initialize_sensor_model(ctx); + } + + // Draw fresh white-noise samples. + ctx.magNoise_T = draw_uniform_noise(ctx.rng, 1e-5); + ctx.gyroNoise_rad_s = draw_uniform_noise(ctx.rng, 0.001); + + const Vec3 truthRates = state_body_rates_rad_s(truth_state); + + ctx.BfieldMeasured_body_T = Btruth_body_T + ctx.magBias_T + ctx.magNoise_T; + ctx.pqrMeasured_rad_s = truthRates + ctx.gyroBias_rad_s + ctx.gyroNoise_rad_s; +} diff --git a/reference_standalone/original_project/src/wmm.cpp b/reference_standalone/original_project/src/wmm.cpp new file mode 100644 index 0000000..a2ada4b --- /dev/null +++ b/reference_standalone/original_project/src/wmm.cpp @@ -0,0 +1,326 @@ +#include "wmm.hpp" +#include +#include + +namespace { + +constexpr int kMaxOrd = 12; +constexpr int kSize = kMaxOrd + 1; +constexpr double kEpoch = 2025.0; + +struct WmmCoeff { + int n; + int m; + double g; + double h; + double gd; + double hd; +}; + +// Embedded WMM2025 coefficients. +// Keeping them in the code means the sim runs without any extra data files. +constexpr WmmCoeff kCoeff[] = { + {1,0,-29351.8,0.0,12.0,0.0}, + {1,1,-1410.8,4545.4,9.7,-21.5}, + {2,0,-2556.6,0.0,-11.6,0.0}, + {2,1,2951.1,-3133.6,-5.2,-27.7}, + {2,2,1649.3,-815.1,-8.0,-12.1}, + {3,0,1361.0,0.0,-1.3,0.0}, + {3,1,-2404.1,-56.6,-4.2,4.0}, + {3,2,1243.8,237.5,0.4,-0.3}, + {3,3,453.6,-549.5,-15.6,-4.1}, + {4,0,895.0,0.0,-1.6,0.0}, + {4,1,799.5,278.6,-2.4,-1.1}, + {4,2,55.7,-133.9,-6.0,4.1}, + {4,3,-281.1,212.0,5.6,1.6}, + {4,4,12.1,-375.6,-7.0,-4.4}, + {5,0,-233.2,0.0,0.6,0.0}, + {5,1,368.9,45.4,1.4,-0.5}, + {5,2,187.2,220.2,0.0,2.2}, + {5,3,-138.7,-122.9,0.6,0.4}, + {5,4,-142.0,43.0,2.2,1.7}, + {5,5,20.9,106.1,0.9,1.9}, + {6,0,64.4,0.0,-0.2,0.0}, + {6,1,63.8,-18.4,-0.4,0.3}, + {6,2,76.9,16.8,0.9,-1.6}, + {6,3,-115.7,48.8,1.2,-0.4}, + {6,4,-40.9,-59.8,-0.9,0.9}, + {6,5,14.9,10.9,0.3,0.7}, + {6,6,-60.7,72.7,0.9,0.9}, + {7,0,79.5,0.0,-0.0,0.0}, + {7,1,-77.0,-48.9,-0.1,0.6}, + {7,2,-8.8,-14.4,-0.1,0.5}, + {7,3,59.3,-1.0,0.5,-0.8}, + {7,4,15.8,23.4,-0.1,0.0}, + {7,5,2.5,-7.4,-0.8,-1.0}, + {7,6,-11.1,-25.1,-0.8,0.6}, + {7,7,14.2,-2.3,0.8,-0.2}, + {8,0,23.2,0.0,-0.1,0.0}, + {8,1,10.8,7.1,0.2,-0.2}, + {8,2,-17.5,-12.6,0.0,0.5}, + {8,3,2.0,11.4,0.5,-0.4}, + {8,4,-21.7,-9.7,-0.1,0.4}, + {8,5,16.9,12.7,0.3,-0.5}, + {8,6,15.0,0.7,0.2,-0.6}, + {8,7,-16.8,-5.2,-0.0,0.3}, + {8,8,0.9,3.9,0.2,0.2}, + {9,0,4.6,0.0,-0.0,0.0}, + {9,1,7.8,-24.8,-0.1,-0.3}, + {9,2,3.0,12.2,0.1,0.3}, + {9,3,-0.2,8.3,0.3,-0.3}, + {9,4,-2.5,-3.3,-0.3,0.3}, + {9,5,-13.1,-5.2,0.0,0.2}, + {9,6,2.4,7.2,0.3,-0.1}, + {9,7,8.6,-0.6,-0.1,-0.2}, + {9,8,-8.7,0.8,0.1,0.4}, + {9,9,-12.9,10.0,-0.1,0.1}, + {10,0,-1.3,0.0,0.1,0.0}, + {10,1,-6.4,3.3,0.0,0.0}, + {10,2,0.2,0.0,0.1,-0.0}, + {10,3,2.0,2.4,0.1,-0.2}, + {10,4,-1.0,5.3,-0.0,0.1}, + {10,5,-0.6,-9.1,-0.3,-0.1}, + {10,6,-0.9,0.4,0.0,0.1}, + {10,7,1.5,-4.2,-0.1,0.0}, + {10,8,0.9,-3.8,-0.1,-0.1}, + {10,9,-2.7,0.9,-0.0,0.2}, + {10,10,-3.9,-9.1,-0.0,-0.0}, + {11,0,2.9,0.0,0.0,0.0}, + {11,1,-1.5,0.0,-0.0,-0.0}, + {11,2,-2.5,2.9,0.0,0.1}, + {11,3,2.4,-0.6,0.0,-0.0}, + {11,4,-0.6,0.2,0.0,0.1}, + {11,5,-0.1,0.5,-0.1,-0.0}, + {11,6,-0.6,-0.3,0.0,-0.0}, + {11,7,-0.1,-1.2,-0.0,0.1}, + {11,8,1.1,-1.7,-0.1,-0.0}, + {11,9,-1.0,-2.9,-0.1,0.0}, + {11,10,-0.2,-1.8,-0.1,0.0}, + {11,11,2.6,-2.3,-0.1,0.0}, + {12,0,-2.0,0.0,0.0,0.0}, + {12,1,-0.2,-1.3,0.0,-0.0}, + {12,2,0.3,0.7,-0.0,0.0}, + {12,3,1.2,1.0,-0.0,-0.1}, + {12,4,-1.3,-1.4,-0.0,0.1}, + {12,5,0.6,-0.0,-0.0,-0.0}, + {12,6,0.6,0.6,0.1,-0.0}, + {12,7,0.5,-0.1,-0.0,-0.0}, + {12,8,-0.1,0.8,0.0,0.0}, + {12,9,-0.4,0.1,0.0,-0.0}, + {12,10,-0.2,-1.0,-0.1,-0.0}, + {12,11,-1.3,0.1,-0.0,0.0}, + {12,12,-0.7,0.2,-0.1,-0.1} +}; + +struct WmmTables { + double c[kSize][kSize]{}; + double cd[kSize][kSize]{}; + double snorm[kSize * kSize]{}; + double fn[kSize]{}; + double fm[kSize]{}; + double k[kSize][kSize]{}; +}; + +const WmmTables& tables() { + static const WmmTables t = [] { + WmmTables out{}; + + // Read the coefficient list into the legacy matrix layout. + for (const auto& entry : kCoeff) { + out.c[entry.m][entry.n] = entry.g; + out.cd[entry.m][entry.n] = entry.gd; + if (entry.m != 0) { + out.c[entry.n][entry.m - 1] = entry.h; + out.cd[entry.n][entry.m - 1] = entry.hd; + } + } + + // Convert Schmidt-normalized coefficients into the unnormalized form used by the old WMM code. + out.snorm[0] = 1.0; + out.fm[0] = 0.0; + + for (int n = 1; n <= kMaxOrd; ++n) { + out.snorm[n] = out.snorm[n - 1] * static_cast(2 * n - 1) / static_cast(n); + + int j = 2; + int m = 0; + while (m <= n) { + if (n > 1) { + out.k[m][n] = static_cast(((n - 1) * (n - 1)) - (m * m)) / + static_cast((2 * n - 1) * (2 * n - 3)); + } + + if (m > 0) { + const double flnmj = static_cast((n - m + 1) * j) / static_cast(n + m); + out.snorm[n + m * kSize] = out.snorm[n + (m - 1) * kSize] * std::sqrt(flnmj); + j = 1; + + out.c[n][m - 1] *= out.snorm[n + m * kSize]; + out.cd[n][m - 1] *= out.snorm[n + m * kSize]; + } + + out.c[m][n] *= out.snorm[n + m * kSize]; + out.cd[m][n] *= out.snorm[n + m * kSize]; + ++m; + } + + out.fn[n] = static_cast(n + 1); + out.fm[n] = static_cast(n); + } + + out.k[1][1] = 0.0; + return out; + }(); + + return t; +} + +} // namespace + +void wmm2025_geodetic_ned_nT(double latitude_deg, + double longitude_deg, + double altitude_km, + double decimal_year, + double& X_nT, + double& Y_nT, + double& Z_nT) { + if (decimal_year < 2025.0 || decimal_year > 2030.0) { + throw std::runtime_error("WMM2025 is only valid from 2025.0 to 2030.0."); + } + + const auto& wmm = tables(); + + // Working arrays used by the legacy WMM recurrence relations. + double tc[kSize][kSize]{}; + double dp[kSize][kSize]{}; + double sp[kSize]{}; + double cp[kSize]{}; + double pp[kSize]{}; + double p[kSize * kSize]{}; + + for (int i = 0; i < kSize * kSize; ++i) { + p[i] = wmm.snorm[i]; + } + + // WGS84 ellipsoid constants used by the legacy WMM code. + const double a = 6378.137; + const double b = 6356.7523142; + const double re = 6371.2; + const double a2 = a * a; + const double b2 = b * b; + const double c2 = a2 - b2; + const double a4 = a2 * a2; + const double b4 = b2 * b2; + const double c4 = a4 - b4; + + const double dt = decimal_year - kEpoch; + + const double rlon = longitude_deg * M_PI / 180.0; + const double rlat = latitude_deg * M_PI / 180.0; + const double srlon = std::sin(rlon); + const double srlat = std::sin(rlat); + const double crlon = std::cos(rlon); + const double crlat = std::cos(rlat); + const double srlat2 = srlat * srlat; + const double crlat2 = crlat * crlat; + + sp[0] = 0.0; + cp[0] = 1.0; + pp[0] = 1.0; + dp[0][0] = 0.0; + sp[1] = srlon; + cp[1] = crlon; + + // Convert geodetic input into the spherical quantities used by the field equations. + const double q = std::sqrt(a2 - c2 * srlat2); + const double q1 = altitude_km * q; + const double q2 = ((q1 + a2) / (q1 + b2)) * ((q1 + a2) / (q1 + b2)); + const double ct = srlat / std::sqrt(q2 * crlat2 + srlat2); + const double st = std::sqrt(1.0 - ct * ct); + const double r2 = (altitude_km * altitude_km) + 2.0 * q1 + (a4 - c4 * srlat2) / (q * q); + const double r = std::sqrt(r2); + const double d = std::sqrt(a2 * crlat2 + b2 * srlat2); + const double ca = (altitude_km + d) / r; + const double sa = c2 * crlat * srlat / (r * d); + + for (int m = 2; m <= kMaxOrd; ++m) { + sp[m] = sp[1] * cp[m - 1] + cp[1] * sp[m - 1]; + cp[m] = cp[1] * cp[m - 1] - sp[1] * sp[m - 1]; + } + + double aor = re / r; + double ar = aor * aor; + double br = 0.0; + double bt = 0.0; + double bp = 0.0; + double bpp = 0.0; + + for (int n = 1; n <= kMaxOrd; ++n) { + ar *= aor; + + int m = 0; + while (m <= n) { + // Compute the unnormalized associated Legendre polynomials and their derivatives. + if (n == m) { + p[n + m * kSize] = st * p[n - 1 + (m - 1) * kSize]; + dp[m][n] = st * dp[m - 1][n - 1] + ct * p[n - 1 + (m - 1) * kSize]; + } else if (n == 1 && m == 0) { + p[n + m * kSize] = ct * p[n - 1 + m * kSize]; + dp[m][n] = ct * dp[m][n - 1] - st * p[n - 1 + m * kSize]; + } else if (n > 1 && n != m) { + if (m > n - 2) { + p[n - 2 + m * kSize] = 0.0; + dp[m][n - 2] = 0.0; + } + p[n + m * kSize] = ct * p[n - 1 + m * kSize] - wmm.k[m][n] * p[n - 2 + m * kSize]; + dp[m][n] = ct * dp[m][n - 1] - st * p[n - 1 + m * kSize] - wmm.k[m][n] * dp[m][n - 2]; + } + + // Time-shift the coefficients from epoch 2025.0 to the requested decimal year. + tc[m][n] = wmm.c[m][n] + dt * wmm.cd[m][n]; + if (m != 0) { + tc[n][m - 1] = wmm.c[n][m - 1] + dt * wmm.cd[n][m - 1]; + } + + // Accumulate the spherical harmonic expansion. + const double par = ar * p[n + m * kSize]; + double temp1 = 0.0; + double temp2 = 0.0; + if (m == 0) { + temp1 = tc[m][n] * cp[m]; + temp2 = tc[m][n] * sp[m]; + } else { + temp1 = tc[m][n] * cp[m] + tc[n][m - 1] * sp[m]; + temp2 = tc[m][n] * sp[m] - tc[n][m - 1] * cp[m]; + } + + bt -= ar * temp1 * dp[m][n]; + bp += wmm.fm[m] * temp2 * par; + br += wmm.fn[n] * temp1 * par; + + // Special handling at the poles. + if (st == 0.0 && m == 1) { + if (n == 1) { + pp[n] = pp[n - 1]; + } else { + pp[n] = ct * pp[n - 1] - wmm.k[m][n] * pp[n - 2]; + } + const double parp = ar * pp[n]; + bpp += wmm.fm[m] * temp2 * parp; + } + + ++m; + } + } + + if (st == 0.0) { + bp = bpp; + } else { + bp /= st; + } + + // Rotate from spherical components into local geodetic north/east/down. + X_nT = -bt * ca - br * sa; + Y_nT = bp; + Z_nT = bt * sa - br * ca; +} diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..6032b2b --- /dev/null +++ b/requirements.txt @@ -0,0 +1,6 @@ +bsk[examples]>=2.9.0 +numpy>=1.24 +pandas>=2.0 +matplotlib>=3.7 +pytest>=7.0 +pybind11>=2.11