diff --git a/docs/source/Support/bskReleaseNotesSnippets/1330-strip-imaging.rst b/docs/source/Support/bskReleaseNotesSnippets/1330-strip-imaging.rst new file mode 100644 index 00000000000..9c2d9cf17c0 --- /dev/null +++ b/docs/source/Support/bskReleaseNotesSnippets/1330-strip-imaging.rst @@ -0,0 +1,5 @@ +- Added :ref:`stripLocation` to support the strip imaging mode. +- Updated :ref:`locationPointing` to include the new strip imaging mode. +- Added new tests in ``test_locationPointing.py`` to verify the new strip imaging mode. +- Created new message :ref:`StripStateMsgPayload` to interface :ref:`locationPointing` with the new :ref:`stripLocation`. +- Added the scenario :ref:`scenarioStripImaging` showing how to perform strip imaging tasks. diff --git a/examples/_default.rst b/examples/_default.rst index 5033689bb6c..7b8f113fb27 100644 --- a/examples/_default.rst +++ b/examples/_default.rst @@ -68,6 +68,7 @@ Attitude Guidance Layered spiral attitude guidance Constrained Attitude Maneuver Guidance Performing Sweeping Maneuvers + Earth Strip Imaging Tasks diff --git a/examples/scenarioStripImaging.py b/examples/scenarioStripImaging.py new file mode 100644 index 00000000000..e6503259325 --- /dev/null +++ b/examples/scenarioStripImaging.py @@ -0,0 +1,946 @@ +# +# ISC License +# +# Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# + +r""" +Overview +-------- + +This scenario demonstrates sequential strip imaging of three ground strips using the +:ref:`stripLocation` and :ref:`locationPointing` modules. A spacecraft in a low-Earth +equatorial orbit images three strips one after another, each with a different orientation +relative to the orbit track: + +1. **Parallel** to the orbit track (constant latitude, varying longitude) +2. **Perpendicular** to the orbit track (varying latitude, constant longitude) +3. **Diagonal** to the orbit track (both latitude and longitude vary) + +The script is found in the folder ``basilisk/examples`` and executed by using:: + + python3 scenarioStripImaging.py + +Each strip is defined by a start and end geodetic coordinate using the :ref:`stripLocation` +module, which propagates a target point along the great-circle arc between the two endpoints +at a configurable acquisition speed. The :ref:`locationPointing` module provides three-degree-of-freedom +attitude guidance: it points the body-fixed boresight axis ``pHat_B`` toward the moving target while +aligning the scanning-line axis ``cHat_B`` orthogonal to the scanning direction. + +Before each imaging phase, a pre-imaging period allows the attitude control system to achieve acceptable +attitude accuracy and/or wait for an acceptable viewing angle. + +Attitude control is implemented with an MRP steering law (:ref:`mrpSteering`), a rate servo +(:ref:`rateServoFullNonlinear`), and three reaction wheels (:ref:`reactionWheelStateEffector`). + +Two imaging constraints are tracked throughout the mission: + +- **Minimum elevation angle** (10 deg): the view angle between the spacecraft and the target + must exceed this threshold. +- **Maximum principal rotation error** (5 deg): the attitude error must remain below this + threshold during imaging. + +Illustration of Simulation Results +---------------------------------- + +:: + + show_plots = True + +The first plot shows the principal rotation error over the full mission. Gray-shaded regions +indicate pre-imaging periods. During imaging phases, green shading marks times +when the error is below the 5 deg threshold and red shading marks times when it is exceeded. + +.. image:: /_images/Scenarios/scenarioStripImaging1.svg + :align: center + +The second plot shows the view angle (elevation) over the full mission. Green shading during +imaging phases indicates the elevation exceeds the 10 deg minimum, while red indicates it +does not. + +.. image:: /_images/Scenarios/scenarioStripImaging2.svg + :align: center + +The third plot shows the norm of the perpendicular velocity component +:math:`\|\mathbf{v}_{\perp}\|`. When this value is small (hatched red region), the boresight +and target velocity are nearly collinear and the :ref:`locationPointing` module falls back to +point-only targeting. + +.. image:: /_images/Scenarios/scenarioStripImaging3.svg + :align: center + +""" + +# import basic libraries +import os +import time +import matplotlib.pyplot as plt +import numpy as np + +# import message declarations +from Basilisk.architecture import messaging +from Basilisk.architecture import astroConstants + +# import FSW Algorithm related support +from Basilisk.fswAlgorithms import locationPointing +from Basilisk.fswAlgorithms import attTrackingError +from Basilisk.fswAlgorithms import mrpSteering +from Basilisk.fswAlgorithms import rateServoFullNonlinear +from Basilisk.fswAlgorithms import rwMotorTorque + +# import simulation related support +from Basilisk.simulation import reactionWheelStateEffector +from Basilisk.simulation import stripLocation +from Basilisk.simulation import simpleNav +from Basilisk.simulation import spacecraft + +# import general simulation support files +from Basilisk.utilities import SimulationBaseClass +from Basilisk.utilities import macros +from Basilisk.utilities import fswSetupRW +from Basilisk.utilities import orbitalMotion +from Basilisk.utilities import simIncludeGravBody +from Basilisk.utilities import simIncludeRW +from Basilisk.utilities import unitTestSupport + +# attempt to import vizard +from Basilisk.utilities import vizSupport +try: + from Basilisk.simulation import vizInterface +except ImportError: + pass + +from Basilisk import __path__ +from Basilisk.utilities import RigidBodyKinematics + +bskPath = __path__[0] +fileName = os.path.basename(os.path.splitext(__file__)[0]) + + +# --------------------------------------------------------------------------- +# Helper functions +# --------------------------------------------------------------------------- + +def get_target_position(t, r_start, r_end, speed=3e-6): + """Interpolate a point along the great-circle arc from r_start to r_end. + t is in nanoseconds. speed is in m/ns (default 3e-6).""" + R_E = 6378.137e3 + r_start = np.array(r_start, dtype=float).reshape(-1) + r_end = np.array(r_end, dtype=float).reshape(-1) + u_start = r_start / np.linalg.norm(r_start) + u_end = r_end / np.linalg.norm(r_end) + dot_product = np.clip(np.dot(u_start, u_end), -1.0, 1.0) + theta = np.arccos(dot_product) + if theta < 1e-6: + return r_start + arc_length = R_E * theta + t_total = arc_length / speed + t_frac = np.clip(t / t_total, 0.0, 1.0) + sin_theta = np.sin(theta) + u_interp = (np.sin((1 - t_frac) * theta) * u_start + + np.sin(t_frac * theta) * u_end) / sin_theta + return (R_E * u_interp).reshape(3, 1) + + +def compute_extended_start(r_start, r_end, pre_imaging_time, speed=3e-6): + """Replicate the C++ newpstart() logic: extend the strip start backwards + along the great circle by pre_imaging_time to get p_start_updated. + + This matches how stripLocation.cpp computes p_start_updated so that + the Vizard tiles align with the actual target propagation. + """ + R_E = 6378.137e3 + p_start = np.array(r_start, dtype=float).flatten() + p_start = p_start / np.linalg.norm(p_start) * R_E + p_end = np.array(r_end, dtype=float).flatten() + p_end = p_end / np.linalg.norm(p_end) * R_E + + dot_val = np.clip(np.dot(p_start, p_end) / (R_E * R_E), -1.0, 1.0) + theta = np.arccos(dot_val) + + if speed <= 0 or pre_imaging_time <= 0 or theta < 1e-12: + return p_start + + length = theta * R_E + line_speed_ratio = length / speed + t = -pre_imaging_time / line_speed_ratio # negative → backwards + + sin_theta = np.sin(theta) + coeff1 = np.sin((1 - t) * theta) / sin_theta + coeff2 = np.sin(t * theta) / sin_theta + interp = coeff1 * p_start + coeff2 * p_end + return interp / np.linalg.norm(interp) * R_E + + +def compute_strip_vertices(r_start, r_end, width=10e3): + """Compute 4 corner vertices of a rectangular strip on the surface.""" + r_start = np.array(r_start).flatten() + r_end = np.array(r_end).flatten() + d_vec = r_end - r_start + d_vec /= np.linalg.norm(d_vec) + mid_vec = (r_start + r_end) / 2.0 + mid_vec /= np.linalg.norm(mid_vec) + p_vec = np.cross(mid_vec, d_vec) + p_vec /= np.linalg.norm(p_vec) + half_width = width / 2.0 + v1 = r_start + half_width * p_vec + v2 = r_start - half_width * p_vec + v3 = r_end + half_width * p_vec + v4 = r_end - half_width * p_vec + return list(v1) + list(v3) + list(v4) + list(v2) + +def compute_scan_line_extremities(r_s, r_e, width=200e3): + """Compute left and right extremity positions of the current scan line.""" + r_s = np.array(r_s).flatten() + r_e = np.array(r_e).flatten() + r_mid = 0.5 * (r_s + r_e) + d_vec = r_e - r_s + d_norm = np.linalg.norm(d_vec) + if d_norm > 1e-12: + d_vec /= d_norm + radial = r_mid / np.linalg.norm(r_mid) + p_vec = np.cross(radial, d_vec) + p_norm = np.linalg.norm(p_vec) + if p_norm > 1e-12: + p_vec /= p_norm + half_width = width / 2.0 + r_left = r_mid + half_width * p_vec + r_right = r_mid - half_width * p_vec + return r_left.tolist(), r_right.tolist() + +def compute_v_perp_norm(att_ref, v_LP_N, pHat_B): + """ + Compute ||v_perp|| = ||pHat_B x v_TP_B|| at each time step. + + When ||v_perp|| is small, pHat_B and v_TP_B are nearly collinear and the + locationPointing module falls back to point-only targeting. + """ + pHat = np.asarray(pHat_B, dtype=float).ravel() + norms = np.empty(len(att_ref)) + for i in range(len(att_ref)): + sigma_RN_i = att_ref[i] + v_i = v_LP_N[i] + v_mag = np.linalg.norm(v_i) + if v_mag < 1e-12: + norms[i] = 0.0 + continue + v_hat = v_i / v_mag + dcm_RN = np.array(RigidBodyKinematics.MRP2C(sigma_RN_i)) + v_TP_B = dcm_RN @ v_hat + v_TP_B_mag = np.linalg.norm(v_TP_B) + if v_TP_B_mag > 1e-12: + v_TP_B /= v_TP_B_mag + v_perp = np.cross(pHat, v_TP_B) + norms[i] = np.linalg.norm(v_perp) + return norms + +def _add_threshold_shading(ax, imaging_regions_min, threshold, green_below=True): + """Add green/red threshold shading to each imaging region.""" + y_min, y_max = ax.get_ylim() + if y_max - y_min < 1e-12: + return + for (t0, t1) in imaging_regions_min: + if green_below: + # green below threshold (good), red above (bad) – for rotation error + if y_max <= threshold: + ax.axvspan(t0, t1, ymin=0, ymax=1, color="green", alpha=0.14, zorder=0) + elif y_min >= threshold: + ax.axvspan(t0, t1, ymin=0, ymax=1, color="red", alpha=0.14, zorder=0) + else: + frac = (threshold - y_min) / (y_max - y_min) + ax.axvspan(t0, t1, ymin=0, ymax=frac, color="green", alpha=0.14, zorder=0) + ax.axvspan(t0, t1, ymin=frac, ymax=1, color="red", alpha=0.14, zorder=0) + else: + # green above threshold (good), red below (bad) – for view angle + if y_min >= threshold: + ax.axvspan(t0, t1, ymin=0, ymax=1, color="green", alpha=0.14, zorder=0) + elif y_max <= threshold: + ax.axvspan(t0, t1, ymin=0, ymax=1, color="red", alpha=0.14, zorder=0) + else: + frac = (threshold - y_min) / (y_max - y_min) + ax.axvspan(t0, t1, ymin=0, ymax=frac, color="red", alpha=0.14, zorder=0) + ax.axvspan(t0, t1, ymin=frac, ymax=1, color="green", alpha=0.14, zorder=0) + + +def _plot_segments(ax, time_min, data, seg_indices, strip_colors, strip_labels, + gray_indices): + """Plot per-phase colored segments and transition segments.""" + # Transition segments – use the color of the strip they lead into + for k, (i0, i1) in enumerate(gray_indices): + ax.plot(time_min[i0:i1+1], data[i0:i1+1], + color=strip_colors[k], ls="--", lw=1.0) + # Strip segments + for k, (i0, i1) in enumerate(seg_indices): + ax.plot(time_min[i0:i1+1], data[i0:i1+1], + color=strip_colors[k], label=strip_labels[k], lw=1.2) + +def plot_principal_rotation_error(time_min, dataSigmaBR, + gray_regions_min, imaging_regions_min, + seg_indices, gray_indices, + strip_colors, strip_labels, + maxPrincipalRotationError=5.0): + """Plot principal rotation error over the entire sequential mission.""" + sigma_norm = np.linalg.norm(dataSigmaBR, axis=1) + theta_deg = np.degrees(4.0 * np.arctan(sigma_norm)) + + fig, ax = plt.subplots(figsize=(8, 4)) + _plot_segments(ax, time_min, theta_deg, seg_indices, strip_colors, + strip_labels, gray_indices) + + ax.set_ylim(bottom=0.0) + ax.set_xlim(time_min[0], time_min[-1]) + + # Gray shading for pre-imaging / transitions + for (t0, t1) in gray_regions_min: + ax.axvspan(t0, t1, color="gray", alpha=0.15, zorder=0) + + # Green/red threshold shading for imaging periods + _add_threshold_shading(ax, imaging_regions_min, threshold=maxPrincipalRotationError, + green_below=True) + + ax.set_xlabel("Time [min]", fontsize=12) + ax.set_ylabel("Principal Rotation Error [deg]", fontsize=12) + ax.grid(True) + ax.legend(fontsize=10) + plt.tight_layout() + return fig + + +def plot_view_angle(time_min, elevation_rad, + gray_regions_min, imaging_regions_min, + seg_indices, gray_indices, + strip_colors, strip_labels, + minimumElevation_deg=10.0): + """Plot view angle (elevation) over the entire sequential mission.""" + elevation_deg = np.degrees(elevation_rad) + + fig, ax = plt.subplots(figsize=(8, 4)) + _plot_segments(ax, time_min, elevation_deg, seg_indices, strip_colors, + strip_labels, gray_indices) + + ax.set_xlim(time_min[0], time_min[-1]) + + for (t0, t1) in gray_regions_min: + ax.axvspan(t0, t1, color="gray", alpha=0.15, zorder=0) + + # Green/red threshold shading for imaging periods + _add_threshold_shading(ax, imaging_regions_min, threshold=minimumElevation_deg, + green_below=False) + + ax.set_xlabel("Time [min]", fontsize=12) + ax.set_ylabel("View Angle [deg]", fontsize=12) + ax.grid(True) + ax.legend(fontsize=10) + plt.tight_layout() + return fig + + +def plot_v_perp_norm(time_min, v_perp_norms, + gray_regions_min, + seg_indices, gray_indices, + strip_colors, strip_labels, + threshold=0.1): + """Plot ||v_perp|| over the entire sequential mission.""" + fig, ax = plt.subplots(figsize=(8, 4)) + _plot_segments(ax, time_min, v_perp_norms, seg_indices, strip_colors, + strip_labels, gray_indices) + + ax.set_xlim(time_min[0], time_min[-1]) + ax.set_ylim(bottom=0.0) + + # Hatched area below threshold (point-only targeting region) + ax.axhspan(0, threshold, facecolor="red", alpha=0.2, hatch="//", + edgecolor="red", linewidth=0.0, zorder=0, + label="Point Imaging Region") + + for (t0, t1) in gray_regions_min: + ax.axvspan(t0, t1, color="gray", alpha=0.15, zorder=0) + + ax.set_xlabel("Time [min]", fontsize=12) + ax.set_ylabel(r"$\|\mathbf{v}_{\perp}\|$", fontsize=12) + ax.grid(True) + ax.legend(fontsize=10) + plt.tight_layout() + return fig + + +# --------------------------------------------------------------------------- +# Sequential three-strip simulation +# --------------------------------------------------------------------------- + +def run(show_plots=True, enable_viz=False): + """ + Run a single continuous simulation imaging three strips sequentially: + 1. Parallel to orbit track (constant latitude, varying longitude) + 2. Perpendicular to orbit track (varying latitude, constant longitude) + 3. Diagonal to orbit track (both latitude and longitude vary) + + Returns a dict with the full-mission logged data. + """ + + # ================================================================== + # Timing + # ================================================================== + pre_imaging_s = 120 # pre-imaging [s] + imaging_s = 167 # imaging time per strip (500km strip with acquisition speed of 3km/s) [s] + + pre_imaging_ns = macros.sec2nano(pre_imaging_s) + imaging_ns = macros.sec2nano(imaging_s) + + # Phase end-times (cumulative nanoseconds) + phase1_img_start = pre_imaging_ns + phase1_end = phase1_img_start + imaging_ns + + phase2_img_start = phase1_end + pre_imaging_ns + phase2_end = phase2_img_start + imaging_ns + + phase3_img_start = phase2_end + pre_imaging_ns + phase3_end = phase3_img_start + imaging_ns + + # Strip definitions + strip_configs = [ + {"label": "Parallel", "start": (-5, 110), "end": (-5, 120), "color": "blue"}, + {"label": "Perpendicular", "start": (2, 137), "end": (12, 137), "color": "magenta"}, + {"label": "Diagonal", "start": (-1.5, 152), "end": (1.5, 142), "color": "orangered"}, + ] + + # ================================================================== + # Simulation setup + # ================================================================== + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + + dynProcess = scSim.CreateNewProcess(simProcessName) + simulationTimeStep = macros.sec2nano(0.1) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + # ------------------------------------------------------------------ + # Spacecraft + # ------------------------------------------------------------------ + scObject = spacecraft.Spacecraft() + scObject.ModelTag = "spacecraftBody" + I = [100., 0., 0., + 0., 100., 0., + 0., 0., 100.] + scObject.hub.mHub = 330.0 + scObject.hub.r_BcB_B = [[0.0], [0.0], [0.0]] + scObject.hub.IHubPntBc_B = unitTestSupport.np2EigenMatrix3d(I) + + # ------------------------------------------------------------------ + # Gravity + # ------------------------------------------------------------------ + gravFactory = simIncludeGravBody.gravBodyFactory() + earth = gravFactory.createEarth() + earth.isCentralBody = True + mu = earth.mu + gravFactory.addBodiesTo(scObject) + + # ------------------------------------------------------------------ + # Reaction wheels + # ------------------------------------------------------------------ + rwFactory = simIncludeRW.rwFactory() + for axis, omega in zip([[1,0,0],[0,1,0],[0,0,1]], [500., 500., 500.]): + rwFactory.create('Honeywell_HR16', axis, + maxMomentum=50., Omega=omega, u_max=0.5) + numRW = rwFactory.getNumOfDevices() + + rwStateEffector = reactionWheelStateEffector.ReactionWheelStateEffector() + rwFactory.addToSpacecraft(scObject.ModelTag, rwStateEffector, scObject) + + scSim.AddModelToTask(simTaskName, rwStateEffector, 2) + scSim.AddModelToTask(simTaskName, scObject, 1) + + # ------------------------------------------------------------------ + # Navigation + # ------------------------------------------------------------------ + sNavObject = simpleNav.SimpleNav() + sNavObject.ModelTag = "SimpleNavigation" + scSim.AddModelToTask(simTaskName, sNavObject) + + # ------------------------------------------------------------------ + # Three strip targets (all added to the task from the start) + # ------------------------------------------------------------------ + # Imaging constraints (used for Vizard tile colouring and plot shading) + minimumElevation = np.radians(10.0) # view-angle threshold [rad] + maxPrincipalRotationError = 5.0 # attitude error threshold [deg] + + strip_targets = [] + for i, cfg in enumerate(strip_configs): + st = stripLocation.StripLocation() + st.ModelTag = f"Strip_{cfg['label']}" + st.planetRadius = astroConstants.REQ_EARTH * 1e3 + st.acquisition_speed = 3e-6 + st.pre_imaging_time = pre_imaging_ns + st.specifyLocationStart( + np.radians(cfg["start"][0]), np.radians(cfg["start"][1]), 0) + st.specifyLocationEnd( + np.radians(cfg["end"][0]), np.radians(cfg["end"][1]), 0) + st.minimumElevation = minimumElevation + st.maximumRange = 1e9 + st.addSpacecraftToModel(scObject.scStateOutMsg) + scSim.AddModelToTask(simTaskName, st) + strip_targets.append(st) + + # ------------------------------------------------------------------ + # Guidance – location pointing + # ------------------------------------------------------------------ + locPoint = locationPointing.locationPointing() + locPoint.ModelTag = "locPoint" + locPoint.pHat_B = [0, 0, 1] # pointing vector + locPoint.cHat_B = [0, 1, 0] # scanning line vector + locPoint.useBoresightRateDamping = 0 # not needed 3 degrees of control for strip imaging + scSim.AddModelToTask(simTaskName, locPoint) + + # ------------------------------------------------------------------ + # Attitude tracking error + # ------------------------------------------------------------------ + attError = attTrackingError.attTrackingError() + attError.ModelTag = "attErrorInertial3D" + scSim.AddModelToTask(simTaskName, attError) + + # ------------------------------------------------------------------ + # MRP steering controller + # ------------------------------------------------------------------ + mrpControl = mrpSteering.mrpSteering() + mrpControl.ModelTag = "MRP_Steering" + mrpControl.K1 = 10 + mrpControl.K3 = 3 + mrpControl.ignoreOuterLoopFeedforward = False + mrpControl.omega_max = np.radians(1.8) + scSim.AddModelToTask(simTaskName, mrpControl) + + # ------------------------------------------------------------------ + # Rate servo + # ------------------------------------------------------------------ + servo = rateServoFullNonlinear.rateServoFullNonlinear() + servo.ModelTag = "rate_servo" + servo.Ki = 5 + servo.P = 1000 + servo.integralLimit = 2. / servo.Ki * 0.5 + servo.knownTorquePntB_B = [0., 0., 0.] + scSim.AddModelToTask(simTaskName, servo) + + # ------------------------------------------------------------------ + # RW motor-torque mapping + # ------------------------------------------------------------------ + rwMotorTorqueObj = rwMotorTorque.rwMotorTorque() + rwMotorTorqueObj.ModelTag = "rwMotorTorque" + rwMotorTorqueObj.controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1] + scSim.AddModelToTask(simTaskName, rwMotorTorqueObj) + + # ------------------------------------------------------------------ + # Configuration messages + # ------------------------------------------------------------------ + vehicleConfigOut = messaging.VehicleConfigMsgPayload() + vehicleConfigOut.ISCPntB_B = I + vcMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) + + fswSetupRW.clearSetup() + for key, rw in rwFactory.rwList.items(): + fswSetupRW.create(unitTestSupport.EigenVector3d2np(rw.gsHat_B), + rw.Js, 0.2) + fswRwParamMsg = fswSetupRW.writeConfigMessage() + fswSetupRW.clearSetup() # release SWIG-owned objects before shutdown + + # ------------------------------------------------------------------ + # Message connections (guidance starts with strip 1) + # ------------------------------------------------------------------ + sNavObject.scStateInMsg.subscribeTo(scObject.scStateOutMsg) + locPoint.scAttInMsg.subscribeTo(sNavObject.attOutMsg) + locPoint.scTransInMsg.subscribeTo(sNavObject.transOutMsg) + locPoint.locationstripInMsg.subscribeTo( + strip_targets[0].currentStripStateOutMsg) + attError.attNavInMsg.subscribeTo(sNavObject.attOutMsg) + attError.attRefInMsg.subscribeTo(locPoint.attRefOutMsg) + mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg) + servo.guidInMsg.subscribeTo(attError.attGuidOutMsg) + servo.vehConfigInMsg.subscribeTo(vcMsg) + servo.rwParamsInMsg.subscribeTo(fswRwParamMsg) + servo.rwSpeedsInMsg.subscribeTo(rwStateEffector.rwSpeedOutMsg) + servo.rateSteeringInMsg.subscribeTo(mrpControl.rateCmdOutMsg) + rwMotorTorqueObj.rwParamsInMsg.subscribeTo(fswRwParamMsg) + rwMotorTorqueObj.vehControlInMsg.subscribeTo(servo.cmdTorqueOutMsg) + rwStateEffector.rwMotorCmdInMsg.subscribeTo( + rwMotorTorqueObj.rwMotorTorqueOutMsg) + + # ------------------------------------------------------------------ + # Data logging + # ------------------------------------------------------------------ + samplingTime = macros.sec2nano(3) + + attErrLog = attError.attGuidOutMsg.recorder(samplingTime) + attrefLog = locPoint.attRefOutMsg.recorder(samplingTime) + scSim.AddModelToTask(simTaskName, attErrLog) + scSim.AddModelToTask(simTaskName, attrefLog) + + # Per-strip access and target loggers (all record throughout) + accessLogs = [] + targetLogs = [] + for st in strip_targets: + al = st.accessOutMsgs[-1].recorder(samplingTime) + tl = st.currentStripStateOutMsg.recorder(samplingTime) + scSim.AddModelToTask(simTaskName, al) + scSim.AddModelToTask(simTaskName, tl) + accessLogs.append(al) + targetLogs.append(tl) + + # ------------------------------------------------------------------ + # Initial conditions – equatorial orbit + # ------------------------------------------------------------------ + oe = orbitalMotion.ClassicElements() + oe.a = (6378 + 520) * 1000.0 + oe.e = 0 + oe.i = 0 * macros.D2R + oe.Omega = 110 * macros.D2R + oe.omega = 347.8 * macros.D2R + oe.f = 0 * macros.D2R + rN, vN = orbitalMotion.elem2rv(mu, oe) + scObject.hub.r_CN_NInit = rN + scObject.hub.v_CN_NInit = vN + scObject.hub.sigma_BNInit = [[0], [-0.5], [0]] + scObject.hub.omega_BN_BInit = [[0], [0], [0]] + + # ================================================================== + # Vizard visualisation + # ================================================================== + # RGBA colours – default grey for not-yet-scanned tiles + strip_rgba = [0, 0, 0, 120] # black (imaging region, not yet scanned) + strip_rgba_faint = [0, 0, 0, 30] # black (pre-imaging region, not yet scanned) + scan_line_rgba = [255, 215, 0, 255] # yellow-gold scanning line + + # Constraint-based tile colours (assigned at runtime) + TILE_GREEN = [0, 200, 0, 180] # both constraints met – imaging + TILE_GREEN_FAINT = [0, 200, 0, 60] # both constraints met – pre-imaging + TILE_RED = [200, 0, 0, 180] # constraint violated – imaging + TILE_RED_FAINT = [200, 0, 0, 60] # constraint violated – pre-imaging + + # Phase imaging start/end pairs in nanoseconds for each strip + strip_phase_ns = [ + (phase1_img_start, phase1_end), + (phase2_img_start, phase2_end), + (phase3_img_start, phase3_end), + ] + + viz = None + if vizSupport.vizFound and enable_viz: + viz = vizSupport.enableUnityVisualization( + scSim, simTaskName, scObject, + saveFile=fileName, + oscOrbitColorList=[[255, 255, 255, 255]]) + viz.quadMaps.clear() + + # -- Draw the three full strips as quad-map tiles ---------------- + dt_viz = macros.sec2nano(5) + for si, st in enumerate(strip_targets): + id_base = (si + 1) * 100000 + r_start_orig = np.array(st.r_LP_P_Start).flatten() + r_end = np.array(st.r_LP_P_End).flatten() + # Extend start backwards to match C++ p_start_updated + r_start_ext = compute_extended_start( + r_start_orig, r_end, pre_imaging_ns, speed=3e-6) + + t_tile = 0 + tile_id = id_base + img_start_ns, img_end_ns = strip_phase_ns[si] + while t_tile < (img_end_ns - img_start_ns + pre_imaging_ns): + r_s = unitTestSupport.EigenVector3d2list( + get_target_position(t_tile, r_start_ext, r_end, speed=3e-6)) + r_e = unitTestSupport.EigenVector3d2list( + get_target_position(t_tile + dt_viz, r_start_ext, r_end, speed=3e-6)) + verts = compute_strip_vertices(r_s, r_e, width=200e3) + # Default grey – will be updated green/red during simulation + if t_tile < pre_imaging_ns: + clr = strip_rgba_faint + else: + clr = strip_rgba + vizSupport.addQuadMap(viz, ID=tile_id, + parentBodyName="earth", + vertices=verts, color=clr) + t_tile += dt_viz + tile_id += 1 + + # Add two locations for the left and right extremities of the scan line + r_s0 = np.array(strip_targets[0].r_LP_P_Start).flatten() + r_e0 = np.array(strip_targets[0].r_LP_P_End).flatten() + r_s0_ext = compute_extended_start(r_s0, r_e0, pre_imaging_ns, speed=3e-6) + r_init = unitTestSupport.EigenVector3d2list( + get_target_position(0, r_s0_ext, r_e0, speed=3e-6)) + r_init_next = unitTestSupport.EigenVector3d2list( + get_target_position(dt_viz, r_s0_ext, r_e0, speed=3e-6)) + r_left_init, r_right_init = compute_scan_line_extremities( + r_init, r_init_next, width=200e3) + + vizSupport.addLocation(viz, + stationName="Left Extremity", + parentBodyName="earth", + r_GP_P=r_left_init, + fieldOfView=np.radians(160.0), + color=[255, 215, 0, 255], + range=2000.0 * 1000, + markerScale=1.0, + isHidden=False) + vizSupport.createTargetLine(viz, + toBodyName="Left Extremity", + lineColor=[255, 215, 0, 255]) + + vizSupport.addLocation(viz, + stationName="Right Extremity", + parentBodyName="earth", + r_GP_P=r_right_init, + fieldOfView=np.radians(160.0), + color=[255, 215, 0, 255], + range=2000.0 * 1000, + markerScale=1.0, + isHidden=False) + vizSupport.createTargetLine(viz, + toBodyName="Right Extremity", + lineColor=[255, 215, 0, 255]) + + viz.settings.showLocationCommLines = -1 + viz.settings.showLocationCones = -1 + viz.settings.showLocationLabels = -1 + viz.settings.spacecraftSizeMultiplier = 2 + viz.settings.spacecraftCSon = 1 + viz.settings.planetCSon = -1 + viz.settings.celestialBodyOrbitLineWidth = 5 + viz.settings.mainCameraTarget = "spacecraftBody" # target the satellite + viz.settings.forceStartAtSpacecraftLocalView = 1 # start in its local view + # ================================================================== + # Run the simulation in three phases, switching strips between them + # ================================================================== + scSim.InitializeSimulation() + t_total_start = time.perf_counter() + + # Scanning-line quad-map ID base (must not overlap with pre-created strip tile IDs) + SCAN_LINE_ID = 900000 + # Tile step size must match the dt_viz used when pre-creating tiles + dt_tile = macros.sec2nano(5) + + def _step_phase(phase_start_ns, phase_end_ns, strip_idx): + """Execute a phase in 5-second steps, updating the scanning line.""" + dt_step = macros.sec2nano(5) + t_cur = phase_start_ns + st = strip_targets[strip_idx] + r_start_orig = np.array(st.r_LP_P_Start).flatten() + r_end = np.array(st.r_LP_P_End).flatten() + # Use the same extended start as the C++ module + r_start_ext = compute_extended_start( + r_start_orig, r_end, pre_imaging_ns, speed=3e-6) + img_start_ns = strip_phase_ns[strip_idx][0] + id_base = (strip_idx + 1) * 100000 + + while t_cur < phase_end_ns: + t_next = min(t_cur + dt_step, phase_end_ns) + scSim.ConfigureStopTime(t_next) + scSim.ExecuteSimulation() + + # Update scanning line if Vizard is active + if viz is not None: + # Time within the strip's own timeline + elapsed = t_cur - img_start_ns + pre_imaging_ns + r_s = unitTestSupport.EigenVector3d2list( + get_target_position(elapsed, r_start_ext, r_end, speed=3e-6)) + r_e = unitTestSupport.EigenVector3d2list( + get_target_position(elapsed + dt_step, r_start_ext, r_end, speed=3e-6)) + verts = compute_strip_vertices(r_s, r_e, width=200e3) + + # --- Colour the strip tile under the scan line ------- + # Check both imaging constraints + sigma_BR = attError.attGuidOutMsg.read().sigma_BR + sigma_norm = np.linalg.norm(sigma_BR) + pre_deg = np.degrees(4.0 * np.arctan(sigma_norm)) + elev_rad = st.accessOutMsgs[-1].read().elevation + constraints_met = (pre_deg < maxPrincipalRotationError) and (elev_rad > minimumElevation) + is_pre_imaging = (elapsed < pre_imaging_ns) + + if constraints_met: + tile_clr = TILE_GREEN_FAINT if is_pre_imaging else TILE_GREEN + else: + tile_clr = TILE_RED_FAINT if is_pre_imaging else TILE_RED + + # Determine which pre-created tile ID to update + tile_index = int(elapsed // dt_tile) + vizSupport.addQuadMap(viz, ID=id_base + tile_index, + parentBodyName="earth", + vertices=compute_strip_vertices(r_s, r_e, width=200e3), + color=tile_clr) + + # Draw scan line last; nudge vertices ~100 m outward to prevent + # z-fighting with the tile (both quads share the same surface plane) + LIFT = 1.0 + 100.0 / 6.371e6 + scan_verts = [c * LIFT for c in verts] + vizSupport.addQuadMap(viz, ID=SCAN_LINE_ID, + parentBodyName="earth", + vertices=scan_verts, + color=scan_line_rgba) + + # Update extremity location positions + r_left, r_right = compute_scan_line_extremities( + r_s, r_e, width=200e3) + vizSupport.changeLocation(viz, stationName="Left Extremity", r_GP_P=r_left) + vizSupport.changeLocation(viz, stationName="Right Extremity", r_GP_P=r_right) + t_cur = t_next + + # Phase 1: pre-imaging + strip 1 imaging + print("Phase 1: Pre-imaging + Strip 1 (Parallel) ...") + t1_start = time.perf_counter() + _step_phase(0, phase1_end, strip_idx=0) + t1_elapsed = time.perf_counter() - t1_start + print(f" -> Phase 1 done in {t1_elapsed:.2f} s") + + # Switch guidance to strip 2 and reset its imaging clock + print("Phase 2: Transition + Strip 2 (Perpendicular) ...") + locPoint.locationstripInMsg.subscribeTo( + strip_targets[1].currentStripStateOutMsg) + strip_targets[1].duration_strip_imaging = 0 + t2_start = time.perf_counter() + _step_phase(phase1_end, phase2_end, strip_idx=1) + t2_elapsed = time.perf_counter() - t2_start + print(f" -> Phase 2 done in {t2_elapsed:.2f} s") + + # Switch guidance to strip 3 and reset its imaging clock + print("Phase 3: Transition + Strip 3 (Diagonal) ...") + locPoint.locationstripInMsg.subscribeTo( + strip_targets[2].currentStripStateOutMsg) + strip_targets[2].duration_strip_imaging = 0 + t3_start = time.perf_counter() + _step_phase(phase2_end, phase3_end, strip_idx=2) + t3_elapsed = time.perf_counter() - t3_start + print(f" -> Phase 3 done in {t3_elapsed:.2f} s") + + t_total = time.perf_counter() - t_total_start + print("-" * 40) + print(f" Phase 1 : {t1_elapsed:6.2f} s ({100*t1_elapsed/t_total:.1f}%)") + print(f" Phase 2 : {t2_elapsed:6.2f} s ({100*t2_elapsed/t_total:.1f}%)") + print(f" Phase 3 : {t3_elapsed:6.2f} s ({100*t3_elapsed/t_total:.1f}%)") + print(f" Total : {t_total:6.2f} s") + print("-" * 40) + + # ================================================================== + # Retrieve and stitch logged data + # ================================================================== + times_ns = attErrLog.times() + time_min = times_ns * macros.NANO2MIN + dataSigmaBR = attErrLog.sigma_BR + att_ref = attrefLog.sigma_RN + + N = len(times_ns) + elevation = np.empty(N) + v_LP_N = np.empty((N, 3)) + + for i in range(N): + t = times_ns[i] + if t <= phase1_end: + idx = 0 + elif t <= phase2_end: + idx = 1 + else: + idx = 2 + elevation[i] = accessLogs[idx].elevation[i] + v_LP_N[i] = targetLogs[idx].v_LP_N[i] + + v_perp_norms = compute_v_perp_norm(att_ref, v_LP_N, locPoint.pHat_B) + + # ================================================================== + # Compute phase boundaries in minutes for plotting + # ================================================================== + pre_img_min = pre_imaging_ns * macros.NANO2MIN + p1_end_min = phase1_end * macros.NANO2MIN + p2_img_start_min = phase2_img_start * macros.NANO2MIN + p2_end_min = phase2_end * macros.NANO2MIN + p3_img_start_min = phase3_img_start * macros.NANO2MIN + p3_end_min = phase3_end * macros.NANO2MIN + + # Gray (pre-imaging / transition) regions + gray_regions_min = [ + (time_min[0], pre_img_min), # initial pre-imaging + (p1_end_min, p2_img_start_min), # transition strip 1 → 2 + (p2_end_min, p3_img_start_min), # transition strip 2 → 3 + ] + + # Imaging regions (receive green/red threshold shading) + imaging_regions_min = [ + (pre_img_min, p1_end_min), + (p2_img_start_min, p2_end_min), + (p3_img_start_min, p3_end_min), + ] + + # Index ranges for colored curve segments + def _idx(t_ns): + return int(np.searchsorted(times_ns, t_ns, side="right")) - 1 + + idx_pre_img = _idx(pre_imaging_ns) + idx_p1_end = _idx(phase1_end) + idx_p2_img = _idx(phase2_img_start) + idx_p2_end = _idx(phase2_end) + idx_p3_img = _idx(phase3_img_start) + idx_p3_end = len(times_ns) - 1 + + # Gray segments: pre-imaging, transition 1→2, transition 2→3 + gray_indices = [ + (0, idx_pre_img), + (idx_p1_end, idx_p2_img), + (idx_p2_end, idx_p3_img), + ] + # Imaging segments (one per strip) + seg_indices = [ + (idx_pre_img, idx_p1_end), + (idx_p2_img, idx_p2_end), + (idx_p3_img, idx_p3_end), + ] + strip_colors = [cfg["color"] for cfg in strip_configs] + strip_labels = [cfg["label"] for cfg in strip_configs] + + # ================================================================== + # Plot + # ================================================================== + plt.close("all") + + figureList = {} + figureList[fileName + "1"] = plot_principal_rotation_error( + time_min, dataSigmaBR, + gray_regions_min, imaging_regions_min, + seg_indices, gray_indices, + strip_colors, strip_labels, + maxPrincipalRotationError=maxPrincipalRotationError, + ) + + figureList[fileName + "2"] = plot_view_angle( + time_min, elevation, + gray_regions_min, imaging_regions_min, + seg_indices, gray_indices, + strip_colors, strip_labels, + minimumElevation_deg=np.degrees(minimumElevation), + ) + + figureList[fileName + "3"] = plot_v_perp_norm( + time_min, v_perp_norms, + gray_regions_min, + seg_indices, gray_indices, + strip_colors, strip_labels, + ) + + if show_plots: + plt.show() + + # close the plots being saved off to avoid over-writing old and new figures + plt.close("all") + + return figureList + + +if __name__ == "__main__": + run(show_plots=True) diff --git a/src/architecture/msgPayloadDefC/StripStateMsgPayload.h b/src/architecture/msgPayloadDefC/StripStateMsgPayload.h new file mode 100644 index 00000000000..73bf43c5ed6 --- /dev/null +++ b/src/architecture/msgPayloadDefC/StripStateMsgPayload.h @@ -0,0 +1,35 @@ +/* + ISC License + + Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef STRIPSTATE_MSG_H +#define STRIPSTATE_MSG_H + +/*! @brief Message that defines the inertial location of the current target groundlocation on the central line of the strip and its velocity + */ +typedef struct { + double r_LN_N[3]; //!< Position vector of the current target w.r.t. the inertial origin in the inertial frame + double r_LP_N[3]; //!< Position vector of the current target with respect to the planet center in the inertial frame + double v_LP_N[3]; //!< Velocity vector of the current target relative to the planet in the inertial frame + double v_LN_N[3]; //!< Velocity vector of the current target relative to the inertial origin in the inertial frame + +}StripStateMsgPayload; + + + +#endif //STRIPSTATE_MSG_H diff --git a/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py b/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py index 3bbcc6ee635..d27b0a701e3 100644 --- a/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py +++ b/src/fswAlgorithms/attGuidance/locationPointing/_UnitTest/test_locationPointing.py @@ -1,7 +1,7 @@ # # ISC License # -# Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado Boulder +# Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado Boulder # # Permission to use, copy, modify, and/or distribute this software for any # purpose with or without fee is hereby granted, provided that the above @@ -17,11 +17,7 @@ # # -# Unit Test Script -# Module Name: locationPointing -# Author: Hanspeter Schaub -# Creation Date: May 9, 2021 -# + import math import numpy as np @@ -36,37 +32,49 @@ @pytest.mark.parametrize("accuracy", [1e-12]) @pytest.mark.parametrize("r_LS_N", [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.], [0, -1, 0.], [1, 1, 1]]) -@pytest.mark.parametrize("locationType", [0, 1, 2]) +@pytest.mark.parametrize("locationType, v_LP_N", [ + (0, [0., 0., 0.]), + (1, [0., 0., 0.]), + (2, [0., 0., 0.]), + (3, [0., 1., 0.]), + (3, [0., 0., 1.]), + (3, [1., 0., 0.]), +]) @pytest.mark.parametrize("use3DRate", [True, False]) -def test_locationPointing(show_plots, r_LS_N, locationType, use3DRate, accuracy): +def test_locationPointing(show_plots, r_LS_N, locationType, v_LP_N, use3DRate, accuracy): r""" **Validation Test Description** This unit test ensures that the Attitude Guidance and Attitude Reference messages content are properly computed - for a series of desired inertial target locations - + for a series of desired inertial target locations. + For strip imaging (locationType=3), the test also verifies that the extra rotation aligns ``cHat_B`` + perpendicular to the scanning direction. **Test Parameters** - Discuss the test parameters used. - Args: r_LS_N (float): position vector of location relative to spacecraft - locationType (int): choose whether to use ``locationInMsg``, ``celBodyInMsg`` or ``scTargetInMsg`` + locationType (int): choose whether to use ``locationInMsg``, ``celBodyInMsg``, + ``scTargetInMsg``, or ``locationstripInMsg`` + v_LP_N (float): strip velocity vector relative to the planet (inertial frame), + only used when locationType=3 use3DRate (bool): choose between 2D or 3D rate control accuracy (float): absolute accuracy value used in the validation tests **Description of Variables Being Tested** The script checks the attitude and rate outputs. + For strip imaging, it additionally checks that the reference attitude correctly + constrains the roll degree of freedom so that ``cHat_B`` is perpendicular to the + strip scanning direction. """ - [testResults, testMessage] = locationPointingTestFunction(show_plots, r_LS_N, locationType, - use3DRate, accuracy) + [testResults, testMessage] = locationPointingTestFunction(show_plots, r_LS_N, v_LP_N, + locationType, use3DRate, accuracy) assert testResults < 1, testMessage -def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, accuracy): +def locationPointingTestFunction(show_plots, r_LS_NIn,v_LP_NIn,locationType, use3DRate, accuracy): """Test method""" testFailCount = 0 testMessages = [] @@ -81,17 +89,22 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, # setup pHat_B = np.array([1, 0, 0]) + cHat_B= np.array([0, 1, 0]) r_SN_N = np.array([10, 11, 12]) r_LS_N = np.array(r_LS_NIn) + v_LP_N = np.array(v_LP_NIn) omega_BN_B = np.array([0.001, 0.002, 0.003]) sigma_BN = np.array([0., 0., 0.]) r_LN_N = r_LS_N + r_SN_N + r_PN_N = np.array([5., 6., 7.]) # planet center in inertial frame + r_LP_N_pos = r_LN_N - r_PN_N # target position relative to planet center # setup module to be tested module = locationPointing.locationPointing() module.ModelTag = "locationPointingTag" unitTestSim.AddModelToTask(unitTaskName, module) module.pHat_B = pHat_B + module.cHat_B = cHat_B eps = 0.1 * macros.D2R module.smallAngle = eps if use3DRate: @@ -121,6 +134,13 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, locationInMsgData.r_BN_N = r_LN_N locationInMsg = messaging.NavTransMsg().write(locationInMsgData) module.scTargetInMsg.subscribeTo(locationInMsg) + elif locationType == 3: + locationInMsgData = messaging.StripStateMsgPayload() + locationInMsgData.r_LN_N = r_LN_N + locationInMsgData.r_LP_N = r_LP_N_pos + locationInMsgData.v_LP_N = v_LP_N + locationInMsg = messaging.StripStateMsg().write(locationInMsgData) + module.locationstripInMsg.subscribeTo(locationInMsg) # subscribe input messages to module module.scTransInMsg.subscribeTo(scTransInMsg) @@ -147,8 +167,8 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, counter += 1 truthSigmaBR, truthOmegaBR, truthSigmaRN, truthOmegaRN_B, truthOmegaRN_N = \ - truthValues(pHat_B, r_LN_N, r_SN_N, scAttRec.sigma_BN, scAttRec.omega_BN_B, eps, timeStep, - use3DRate) + truthValues(pHat_B, cHat_B, r_LN_N, r_SN_N, scAttRec.sigma_BN, scAttRec.omega_BN_B, eps, + timeStep, use3DRate, v_LP_N=v_LP_N, r_LP_N_pos=r_LP_N_pos, isStrip=(locationType == 3)) # compare the module results to the truth values for i in range(0, len(truthSigmaBR)): @@ -191,6 +211,16 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, str(attRefOutMsgRec.times()[i] * macros.NANO2SEC) + "sec\n") + # Geometric property checks on the reference attitude + # Use eps (the module's smallAngle threshold) as tolerance because the module + # intentionally zeroes sigma_BR when the pointing error is below smallAngle, + # so the reference attitude can be off by up to ~eps in pointing direction. + geomFailCount, geomMessages = checkReferenceGeometry( + attRefOutMsgRec.sigma_RN, scAttRec.sigma_BN, r_LN_N, r_SN_N, + pHat_B, cHat_B, v_LP_N, isStrip=(locationType == 3), accuracy=eps) + testFailCount += geomFailCount + testMessages.extend(geomMessages) + if testFailCount == 0: print("PASSED: " + module.ModelTag) else: @@ -199,7 +229,79 @@ def locationPointingTestFunction(show_plots, r_LS_NIn, locationType, use3DRate, return [testFailCount, "".join(testMessages)] -def truthValues(pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, dt, use3DRate): +def checkReferenceGeometry(sigma_RN_list, sigma_BN_list, r_LN_N, r_SN_N, + pHat_B, cHat_B, v_LP_N, isStrip=False, accuracy=1e-10): + """Verify geometric properties of the reference attitude. + + For every recorded time step this function checks: + 1. **Pointing constraint** – the body-fixed pointing axis ``pHat_B``, + when expressed in the inertial frame through the reference DCM, + must be aligned with the target direction ``r_LS_N``. + 2. **Scan-line constraint** (strip imaging only) – the body-fixed + cross-track axis ``cHat_B``, when expressed in the inertial frame, + must be perpendicular to the strip scanning direction ``v_LP_N``. + + Args: + sigma_RN_list: recorded reference MRP history (N×3). + sigma_BN_list: recorded body MRP history (N×3), used only for indexing. + r_LN_N: inertial target position [m]. + r_SN_N: inertial spacecraft position [m]. + pHat_B: body-fixed pointing axis. + cHat_B: body-fixed cross-track axis. + v_LP_N: strip velocity in inertial frame. + isStrip: True when strip-imaging mode is active. + accuracy: absolute tolerance for the checks. + + Returns: + (failCount, messages): number of failures and list of error strings. + """ + failCount = 0 + messages = [] + + r_LS_N = np.array(r_LN_N) - np.array(r_SN_N) + rHat_LS_N = r_LS_N / np.linalg.norm(r_LS_N) + + for i in range(len(sigma_RN_list)): + sigma_RN = np.array(sigma_RN_list[i]) + dcm_RN = RigidBodyKinematics.MRP2C(sigma_RN) # maps N -> R (body) + + # --- 1. Pointing check: pHat_B expressed in N must align with rHat_LS_N --- + # dcm_RN maps inertial vectors to body frame, so + # pHat_N = dcm_RN^T . pHat_B + pHat_N = np.transpose(dcm_RN).dot(pHat_B) + crossProd = np.cross(pHat_N, rHat_LS_N) + if np.linalg.norm(crossProd) > accuracy: + failCount += 1 + messages.append( + f"FAILED: Pointing check at index {i}: " + f"|pHat_N x rHat_LS_N| = {np.linalg.norm(crossProd):.2e} " + f"(should be < {accuracy})\n") + + # --- 2. Scan-line check (strip imaging only) --- + if isStrip and np.linalg.norm(v_LP_N) > 1e-12: + v_hat_N = np.array(v_LP_N) / np.linalg.norm(v_LP_N) + # Reproduce the alignmentThreshold check from C code: + # if v_perp = pHat_B x v_TP_B is too small, the C code skips + # the extra strip rotation, so perpendicularity is not guaranteed. + alignmentThreshold = 0.1 # default value in C code + v_TP_B_check = dcm_RN.dot(v_hat_N) + v_TP_B_check = v_TP_B_check / np.linalg.norm(v_TP_B_check) + v_perp_check = np.cross(pHat_B, v_TP_B_check) + if np.linalg.norm(v_perp_check) >= alignmentThreshold: + cHat_N = np.transpose(dcm_RN).dot(cHat_B) + dotProduct = abs(np.dot(cHat_N, v_hat_N)) + if dotProduct > accuracy: + failCount += 1 + messages.append( + f"FAILED: Scan-line perpendicularity check at index {i}: " + f"|cHat_N . vHat| = {dotProduct:.2e} " + f"(should be < {accuracy})\n") + + return failCount, messages + + +def truthValues(pHat_B, cHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, dt, + use3DRate, v_LP_N=None, r_LP_N_pos=None, isStrip=False): # setup eHat180_B eHat180_B = np.cross(pHat_B, np.array([1., 0., 0.])) if np.linalg.norm(eHat180_B) < 0.1: @@ -219,7 +321,10 @@ def truthValues(pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, sigma_BN = sigma_BNList[counter] dcmBN = RigidBodyKinematics.MRP2C(sigma_BN) r_LS_B = dcmBN.dot(r_LS_N) - phi = math.acos(pHat_B.dot(r_LS_B) / np.linalg.norm(r_LS_B)) + dum1 = pHat_B.dot(r_LS_B) / np.linalg.norm(r_LS_B) + if abs(dum1) > 1.0: + dum1 = dum1 / abs(dum1) + phi = math.acos(dum1) if phi < smallAngle: sigma_BR = np.array([0., 0., 0.]) else: @@ -230,12 +335,59 @@ def truthValues(pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, eHat_B = eHat_B / np.linalg.norm(eHat_B) sigma_BR = - math.tan(phi / 4.) * eHat_B + # Compute pointing-only reference attitude sigma_RN + sigma_RN = RigidBodyKinematics.addMRP(sigma_BN, -sigma_BR) + + if isStrip and v_LP_N is not None: + # --- Extra rotation for strip imaging --- + alignmentThreshold = 0.1 # default value in C code + + # Step 1: Transform strip velocity to reference body frame + dcmRN = RigidBodyKinematics.MRP2C(sigma_RN) + v_hat = v_LP_N / np.linalg.norm(v_LP_N) + v_TP_B = dcmRN.dot(v_hat) + v_TP_B = v_TP_B / np.linalg.norm(v_TP_B) + + # Step 2: Compute v_perp = pHat_B x v_TP_B + v_perp = np.cross(pHat_B, v_TP_B) + if np.linalg.norm(v_perp) < alignmentThreshold: + # Special case: v_TP_B and pHat_B are colinear or nearly colinear. + # v_perp is either not defined or unstable (90-degree shifts). + # No extra rotation performed (matches C code goto skipStripRotation). + pass + else: + v_perp = v_perp / np.linalg.norm(v_perp) + + # Step 3: Choose sign of v_perp to minimize rotation from cHat_B + if np.dot(cHat_B, v_perp) < 0.0: + v_perp = -v_perp + + # Step 4: Compute rotation angle from cHat_B to v_perp + dotProd2 = np.dot(cHat_B, v_perp) + if abs(dotProd2) > 1.0: + dotProd2 = dotProd2 / abs(dotProd2) + angle = math.acos(dotProd2) + + # Step 5: Compute sigma_R2R + if angle < smallAngle: + sigma_R2R = np.array([0., 0., 0.]) + else: + eHat_R2R = np.cross(cHat_B, v_perp) + eHat_R2R = eHat_R2R / np.linalg.norm(eHat_R2R) + sigma_R2R = math.tan(angle / 4.) * eHat_R2R + + # Step 6: Compose sigma_R2N = addMRP(sigma_RN, sigma_R2R) + sigma_R2N = RigidBodyKinematics.addMRP(sigma_RN, sigma_R2R) + + # Step 7: Replace sigma_BR with strip-corrected value + sigma_BR = RigidBodyKinematics.subMRP(sigma_BN, sigma_R2N) + if counter >= 1: dsigma = (sigma_BR - sigma_BR_Out[counter - 1]) / dt Binv = RigidBodyKinematics.BinvMRP(sigma_BR) omega_BR_B = Binv.dot(dsigma) * 4 - if use3DRate: + if use3DRate and not isStrip: rHat_LS_B = r_LS_B / np.linalg.norm(r_LS_B) omega_BR_B = omega_BR_B + (omega_BNList[counter].dot(rHat_LS_B))*rHat_LS_B @@ -252,4 +404,4 @@ def truthValues(pHat_B, r_LN_N, r_SN_N, sigma_BNList, omega_BNList, smallAngle, if __name__ == "__main__": - locationPointingTestFunction(False, [1, 0, 0], True, False, 1e-12) + locationPointingTestFunction(False, [1, 0, 0], [0., 1., 0.], 3, False, 1e-12) diff --git a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.c b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.c index 772121d9d29..ba70422c1f6 100644 --- a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.c +++ b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.c @@ -1,7 +1,7 @@ /* ISC License - Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado Boulder + Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado Boulder Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted, provided that the above @@ -57,6 +57,7 @@ void Reset_locationPointing(locationPointingConfig *configData, uint64_t callTim _bskLog(configData->bskLogger, BSK_ERROR, "Error: locationPointing.scTransInMsg was not connected."); } int numInMsgs = GroundStateMsg_C_isLinked(&configData->locationInMsg) + + StripStateMsg_C_isLinked(&configData->locationstripInMsg) + EphemerisMsg_C_isLinked(&configData->celBodyInMsg) + NavTransMsg_C_isLinked(&configData->scTargetInMsg); @@ -69,6 +70,11 @@ void Reset_locationPointing(locationPointingConfig *configData, uint64_t callTim configData->init = 1; + /* set default alignment threshold if not provided by user */ + if (configData->alignmentThreshold <= 0.0) { + configData->alignmentThreshold = 0.1; + } + v3SetZero(configData->sigma_BR_old); configData->time_old = callTime; @@ -89,6 +95,25 @@ void Reset_locationPointing(locationPointingConfig *configData, uint64_t callTim } v3Normalize(configData->eHat180_B, configData->eHat180_B); } + + /* check cHat_B when strip imaging is used */ + if (StripStateMsg_C_isLinked(&configData->locationstripInMsg)) { + if (v3Norm(configData->cHat_B) < 0.1) { + char info[MAX_LOGGING_LENGTH]; + sprintf(info, "locationPointing: vector cHat_B is not setup as a unit vector [%f, %f, %f]", + configData->cHat_B[0], configData->cHat_B[1], configData->cHat_B[2]); + _bskLog(configData->bskLogger, BSK_ERROR, info); + } else { + v3Normalize(configData->cHat_B, configData->cHat_B); /* ensure that this vector is a unit vector */ + double dotPHatCHat = fabs(v3Dot(configData->pHat_B, configData->cHat_B)); + if (dotPHatCHat > 1e-6) { + char info[MAX_LOGGING_LENGTH]; + sprintf(info, "locationPointing: cHat_B is not perpendicular to pHat_B (dot product = %e). " + "These vectors must be orthogonal for strip imaging.", dotPHatCHat); + _bskLog(configData->bskLogger, BSK_ERROR, info); + } + } + } } @@ -104,6 +129,7 @@ void Update_locationPointing(locationPointingConfig *configData, uint64_t callTi NavAttMsgPayload scAttInMsgBuffer; //!< local copy of input message buffer NavTransMsgPayload scTransInMsgBuffer; //!< local copy of input message buffer GroundStateMsgPayload locationInMsgBuffer; //!< local copy of location input message buffer + StripStateMsgPayload locationstripInMsgBuffer; //!< local copy of strip state input message buffer EphemerisMsgPayload celBodyInMsgBuffer; //!< local copy of celestial body input message buffer NavTransMsgPayload scTargetInMsgBuffer; //!< local copy of input message buffer of target spacecraft AttGuidMsgPayload attGuidOutMsgBuffer; //!< local copy of guidance output message buffer @@ -114,38 +140,61 @@ void Update_locationPointing(locationPointingConfig *configData, uint64_t callTi double rHat_LS_B[3]; /*!< unit vector of location w.r.t spacecraft CoM in body frame */ double eHat_B[3]; /*!< --- Eigen Axis */ double dcmBN[3][3]; /*!< inertial spacecraft orientation DCM */ + double dcmBN_update[3][3]; /*!< inertial reference spacecraft orientation DCM after pointing at the target */ double phi; /*!< principal angle between pHat and heading to location */ double sigmaDot_BR[3]; /*!< time derivative of sigma_BR*/ double sigma_BR[3]; /*!< MRP of B relative to R */ double sigma_RB[3]; /*!< MRP of R relative to B */ + double sigma_RN[3]; /*!< MRP of R relative to N */ double omega_RN_B[3]; /*!< angular velocity of the R frame w.r.t the B frame in B frame components */ double difference[3]; double time_diff; /*!< module update time */ double Binv[3][3]; /*!< BinvMRP for dsigma_RB_R calculations*/ double dum1; double r_TN_N[3]; /*!< [m] inertial target location */ + double v_TP_N[3]; /*!< [m] inertial target velocity */ + double v_TP_B[3]; /*!< [m/s] v_TP_N in the reference body frame to point at the target */ double boreRate_B[3]; /*!< [rad/s] rotation rate about target direction */ - + bool imagingstrip; /*!< bool indicating if we are imaging a strip or not*/ + double v_perp[3]; /*!< Perpendicular vector to v_TP_B in the plane perpendicular to pHat_B*/ + double dotProd2; /*!< dot product between cHat_B and v_perp */ + double angle; /*!< [rad] angle between cHat_B and v_perp */ + double sigma_R2R[3]; /*!< MRP of R2 (reference after pointing at the target and being perpendicular to the strip) relative to R (reference after only pointing at the target) */ + double sigma_R2N[3]; /*!< MRP of R2 relative to N */ + double sigma_N2R[3]; + double sigma_BR2[3]; /*!< MRP of B relative to R2 */ + double sigma_R2B[3]; /*!< MRP of R2 relative to B */ + double sigma_NB[3]; + double neg_sigma_R2N[3]; + double r_TP_B[3]; // zero output buffer attGuidOutMsgBuffer = AttGuidMsg_C_zeroMsgPayload(); attRefOutMsgBuffer = AttRefMsg_C_zeroMsgPayload(); - // read in the input messages + // read the input messages scAttInMsgBuffer = NavAttMsg_C_read(&configData->scAttInMsg); scTransInMsgBuffer = NavTransMsg_C_read(&configData->scTransInMsg); if (GroundStateMsg_C_isLinked(&configData->locationInMsg)) { locationInMsgBuffer = GroundStateMsg_C_read(&configData->locationInMsg); v3Copy(locationInMsgBuffer.r_LN_N, r_TN_N); + imagingstrip=false; + } + else if (StripStateMsg_C_isLinked(&configData->locationstripInMsg)) { + locationstripInMsgBuffer = StripStateMsg_C_read(&configData->locationstripInMsg); + v3Copy(locationstripInMsgBuffer.r_LN_N, r_TN_N); + v3Copy(locationstripInMsgBuffer.v_LP_N, v_TP_N); + imagingstrip=true; } else if (EphemerisMsg_C_isLinked(&configData->celBodyInMsg)) { celBodyInMsgBuffer = EphemerisMsg_C_read(&configData->celBodyInMsg); v3Copy(celBodyInMsgBuffer.r_BdyZero_N, r_TN_N); + imagingstrip=false; } else { scTargetInMsgBuffer = NavTransMsg_C_read(&configData->scTargetInMsg); v3Copy(scTargetInMsgBuffer.r_BN_N, r_TN_N); + imagingstrip=false; } - - /* calculate r_LS_N*/ + /* calculate r_LS_N */ v3Subtract(r_TN_N, scTransInMsgBuffer.r_BN_N, r_LS_N); /* principle rotation angle to point pHat at location */ @@ -173,11 +222,70 @@ void Update_locationPointing(locationPointingConfig *configData, uint64_t callTi v3Normalize(eHat_B, eHat_B); v3Scale(-tan(phi / 4.), eHat_B, sigma_BR); } - v3Copy(sigma_BR, attGuidOutMsgBuffer.sigma_BR); - // compute sigma_RN + /* Compute the final reference attitude sigma_RN to only point at the target location */ v3Scale(-1.0, sigma_BR, sigma_RB); - addMRP(scAttInMsgBuffer.sigma_BN, sigma_RB, attRefOutMsgBuffer.sigma_RN); + addMRP(scAttInMsgBuffer.sigma_BN, sigma_RB, sigma_RN); + + /* In case we are not imaging a strip, we have the final reference attitude sigma_RN and the final attitude tracking error sigma_BR */ + if (!imagingstrip){ + v3Copy(sigma_BR, attGuidOutMsgBuffer.sigma_BR); + v3Copy(sigma_RN, attRefOutMsgBuffer.sigma_RN); + } + /* In case we are imaging a strip, we need to perform an extra rotation to image perpendicularly to the scanning direction */ + else{ + //Step 1 : Compute the reference scanning line direction + MRP2C(sigma_RN, dcmBN_update); + v3Normalize(v_TP_N, v_TP_N); + m33MultV3(dcmBN_update, v_TP_N, v_TP_B); + v3Normalize(v_TP_B, v_TP_B); + v3Cross(configData->pHat_B, v_TP_B, v_perp); + if (v3Norm(v_perp) < configData->alignmentThreshold) { + /* Special case where v_TP_B and pHat_B are colinear or almost colinear. + In that case v_perp is either not defined or unstable (90 degrees shifts). + No extra rotation performed. */ + v3Copy(sigma_BR, attGuidOutMsgBuffer.sigma_BR); + v3Copy(sigma_RN, attRefOutMsgBuffer.sigma_RN); + goto skipStripRotation; + } + v3Normalize(v_perp, v_perp); + + // Step 2 : Choose the v_perp sign that minimizes the rotation angle. + // Both +v_perp and -v_perp are valid scanning directions (both perpendicular + // to the strip velocity). Pick the one closer to cHat_B + if (v3Dot(configData->cHat_B, v_perp) < 0.0) { + v3Scale(-1.0, v_perp, v_perp); + } + + // Step 3 : Compute sigma_R2R to align cHat_B with v_perp + dotProd2 = v3Dot(configData->cHat_B, v_perp); + if (fabs(dotProd2) > 1.0) { + dotProd2 = dotProd2 / fabs(dotProd2); + } + angle = safeAcos(dotProd2); + /* calculate sigma_R2R */ + if (angle < configData->smallAngle) { + /* cHat_B and v_perp are essentially aligned. Set rotation to zero. */ + v3SetZero(sigma_R2R); + } else { + double eHat_R2R[3]; // CHat_B and v_perp are not aligned + v3Cross(configData->cHat_B, v_perp, eHat_R2R); + v3Normalize(eHat_R2R, eHat_R2R); + v3Scale(tan(angle / 4.), eHat_R2R, sigma_R2R); + } + + //Step 4 : Compute the final reference attitude sigma_R2N to point at the target location and image perpendicularly to the strip */ + addMRP(sigma_RN, sigma_R2R, sigma_R2N); + v3Copy(sigma_R2N, attRefOutMsgBuffer.sigma_RN); + + //Step 5 : Compute the final attitude tracking error sigma_BR2 + subMRP(scAttInMsgBuffer.sigma_BN,sigma_R2N,sigma_BR2); + v3Copy(sigma_BR2, attGuidOutMsgBuffer.sigma_BR); + + //Step 6 : sigma_BR takes the value of sigma_BR2 to make it compatible with the computation of d(sigma_BR)/dt + v3Copy(sigma_BR2, sigma_BR); + } + skipStripRotation: /* use sigma_BR to compute d(sigma_BR)/dt if at least two data points */ if (configData->init < 1) { @@ -199,7 +307,7 @@ void Update_locationPointing(locationPointingConfig *configData, uint64_t callTi configData->init -= 1; } - if (configData->useBoresightRateDamping) { + if (configData->useBoresightRateDamping && !imagingstrip) { v3Scale(v3Dot(scAttInMsgBuffer.omega_BN_B, rHat_LS_B), rHat_LS_B, boreRate_B); v3Add(attGuidOutMsgBuffer.omega_BR_B, boreRate_B, attGuidOutMsgBuffer.omega_BR_B); } diff --git a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.h b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.h index 7d62e506a8c..df05d4b04ec 100644 --- a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.h +++ b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.h @@ -1,7 +1,7 @@ /* ISC License - Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado Boulder + Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado Boulder Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted, provided that the above @@ -25,10 +25,12 @@ #include "cMsgCInterface/NavAttMsg_C.h" #include "cMsgCInterface/NavTransMsg_C.h" #include "cMsgCInterface/GroundStateMsg_C.h" +#include "cMsgCInterface/StripStateMsg_C.h" #include "cMsgCInterface/AttGuidMsg_C.h" #include "cMsgCInterface/AttRefMsg_C.h" #include "cMsgCInterface/EphemerisMsg_C.h" #include "architecture/utilities/bskLogging.h" +#include /*! @brief This module is used to generate the attitude reference message in order to have a spacecraft point at a location on the ground */ @@ -36,8 +38,10 @@ typedef struct { /* user configurable variables */ double pHat_B[3]; /*!< body fixed vector that is to be aimed at a location */ + double cHat_B[3]; /*!< body fixed vector, perpendicular to pHat by definition. This vector needs to become perpendicular to the central line of the strip (the velocity vector of the target point) */ double smallAngle; /*!< rad An angle value that specifies what is near 0 or 180 degrees */ int useBoresightRateDamping; /*!< [int] flag to use rate damping about the sensor boresight */ + double alignmentThreshold; /*!< threshold for collinearity check between pHat_B and v_TP_B during strip imaging. Default 0.1 */ /* private variables */ double sigma_BR_old[3]; /*!< Older sigma_BR value, stored for finite diff*/ @@ -49,6 +53,7 @@ typedef struct { NavAttMsg_C scAttInMsg; //!< input msg with inertial spacecraft attitude states NavTransMsg_C scTransInMsg; //!< input msg with inertial spacecraft position states GroundStateMsg_C locationInMsg; //!< input msg with location relative to planet + StripStateMsg_C locationstripInMsg; //!< input msg with location relative to planet EphemerisMsg_C celBodyInMsg; //!< input celestial body message NavTransMsg_C scTargetInMsg; //!< input msg with inertial target spacecraft position states AttGuidMsg_C attGuidOutMsg; //!< attitude guidance output message diff --git a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.i b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.i index 916e07b315d..87e43412c3c 100644 --- a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.i +++ b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.i @@ -1,7 +1,7 @@ /* ISC License - Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado Boulder + Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado Boulder Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted, provided that the above @@ -39,6 +39,8 @@ struct NavTransMsg_C; struct NavAttMsg_C; %include "architecture/msgPayloadDefC/GroundStateMsgPayload.h" struct GroundStateMsg_C; +%include "architecture/msgPayloadDefC/StripStateMsgPayload.h" +struct StripStateMsg_C; %include "architecture/msgPayloadDefC/AttGuidMsgPayload.h" struct AttGuidMsg_C; %include "architecture/msgPayloadDefC/AttRefMsgPayload.h" diff --git a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.rst b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.rst index ef3581915ef..e66a4bd5a30 100644 --- a/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.rst +++ b/src/fswAlgorithms/attGuidance/locationPointing/locationPointing.rst @@ -1,8 +1,9 @@ Executive Summary ----------------- -This module generates an attitude guidance message to make a specified spacecraft pointing vector target an inertial location. -This location could be on a planet if this module is connected with :ref:`groundLocation` for example, or it could point -to a celestial object center using :ref:`EphemerisMsgPayload`, or a spacecraft location using :ref:`NavTransMsgPayload`. +This module computes attitude guidance and reference messages for two imaging modes: + +- **Location Pointing Mode**: An attitude guidance message is generated to make a specified spacecraft pointing vector point at an inertial target location. This location can be on a planet if this module is connected with :ref:`GroundStateMsgPayload`, on a celestial object center using :ref:`EphemerisMsgPayload`, or on a spacecraft using :ref:`NavTransMsgPayload`. +- **Strip Imaging Mode**: When connected to a :ref:`StripStateMsgPayload` message, the module first points the body-fixed pointing vector at the moving target on the strip centerline. It then performs an additional rotation about this pointing direction so that a secondary body-fixed vector becomes orthogonal to the strip scanning direction. This secondary rotation is applied only if the pointing vector and the strip scanning direction fulfill the non-collinearity constraint. Message Connection Descriptions ------------------------------- @@ -27,6 +28,9 @@ provides information on what this message is used for. * - locationInMsg - :ref:`GroundStateMsgPayload` - input msg containing the inertial point location of interest + * - locationstripInMsg + - :ref:`StripStateMsgPayload` + - (alternative) input msg from :ref:`stripLocation` containing the inertial position and velocity of the current strip target point * - celBodyInMsg - :ref:`EphemerisMsgPayload` - (alternative) input msg containing the inertial point location of a celestial body of interest @@ -44,9 +48,13 @@ provides information on what this message is used for. Detailed Module Description --------------------------- -The inertial location of interest is given by :math:`{\bf r}_{L/N}` and can be either extracted from ``locationInMsg`` when + +Location Pointing Mode +^^^^^^^^^^^^^^^^^^^^^^ +The inertial location of interest w.r.t the inertial origin is given by :math:`{\bf r}_{L/N}` and can be either extracted from ``locationInMsg`` when a location on a planet is provided, ``celBodyInMsg`` when a celestial body's ephemeris location is provided (for pointing -at the Sun or the Earth), or ``scTargetInMsg`` when pointing at another spacecraft. +at the Sun or the Earth), ``scTargetInMsg`` when pointing at another spacecraft, or ``locationstripInMsg`` when performing +strip imaging. The vector pointing from the satellite location :math:`{\bf r}_{S/N}` to this location is then .. math:: @@ -54,21 +62,21 @@ The vector pointing from the satellite location :math:`{\bf r}_{S/N}` to this lo Let :math:`\hat{\bf r}_{L/S}` be the normalized heading vector to this location. -The unit vector :math:`\hat{\bf p}` is a body-fixed vector and denotes the body axis which is to point towards -the desired location :math:`L`. Thus this modules performs a 2-degree of freedom attitude guidance and +The unit vector :math:`\hat{\bf p}_B` is a body-fixed vector and denotes the body axis which is to point towards +the desired location :math:`L`. Thus this mode performs a 2-degree of freedom attitude guidance and control solution. -The eigen-axis to rotate :math:`\hat{\bf p}` towards :math:`\hat{\bf r}_{L/S}` is given by +The eigen-axis to rotate :math:`\hat{\bf p}_B` towards :math:`\hat{\bf r}_{L/S}` is given by .. math:: - \hat{\bf e} = \frac{\hat{\bf p} \times \hat{\bf r}_{L/S}}{|\hat{\bf p} \times \hat{\bf r}_{L/S}|} + \hat{\bf e} = \frac{\hat{\bf p}_B \times \hat{\bf r}_{L/S}}{|\hat{\bf p}_B \times \hat{\bf r}_{L/S}|} The principle rotation angle :math:`\phi` is .. math:: - \phi = \arccos (\hat{\bf p} \cdot \hat{\bf r}_{L/S} ) + \phi = \arccos (\hat{\bf p}_B \cdot \hat{\bf r}_{L/S} ) The attitude tracking error :math:`{\pmb\sigma}_{B/R}` is then given by @@ -76,39 +84,145 @@ The attitude tracking error :math:`{\pmb\sigma}_{B/R}` is then given by {\pmb\sigma}_{B/R} = - \tan(\phi/4) \hat{\bf e} +The reference attitude :math:`{\pmb\sigma}_{R/N}` is obtained by composing the spacecraft attitude :math:`{\pmb\sigma}_{B/N}` with the tracking error: + +.. math:: + + {\pmb\sigma}_{R/N} = {\pmb\sigma}_{B/N} \oplus {\pmb\sigma}_{R/B} + +where :math:`\oplus` denotes the MRP addition operator. + +When the module is **not** in strip imaging mode the outputs are set directly: + +.. math:: + + {\pmb\sigma}_{B/R}^{\text{out}} &= {\pmb\sigma}_{B/R} \\ + {\pmb\sigma}_{R/N}^{\text{out}} &= {\pmb\sigma}_{R/N} + + +Strip Imaging Mode +^^^^^^^^^^^^^^^^^^ +When the ``locationstripInMsg`` input from the :ref:`stripLocation` module is connected, the module enters +strip imaging mode. The :ref:`StripStateMsgPayload` message provides the inertial position w.r.t to the inertial origin +:math:`{\bf r}_{L/N}` and the inertial velocity w.r.t the planet center +:math:`{\bf v}_{LP/N}` of the current target point moving along the central line of the strip. + +The standard location pointing computation described above is first performed to obtain +:math:`{\pmb\sigma}_{R/N}` (the reference that points :math:`\hat{\bf p}_B` at the current target). +An additional rotation about the pointing axis :math:`\hat{\bf p}_B` is then computed so that a +secondary body-fixed vector :math:`\hat{\bf c}_B` specified by the user and in the plane orthogonal to the pointing axis becomes perpendicular to the strip scanning +direction :math:`{\bf v}_{LP/N}`. This ensures that the instrument's cross-track axis is correctly aligned during image acquisition. + +**Step 1 – Compute the scanning direction in the reference body frame.** +The normalized target velocity :math:`\hat{\bf v}_{LP/N}` is expressed in the reference frame R using the +DCM :math:`[RN]` obtained from :math:`{\pmb\sigma}_{R/N}`: + +.. math:: + + \hat{\bf v}_{LP/R} = [RN] \, \hat{\bf v}_{LP/N} + +The component of :math:`\hat{\bf v}_{LP/R}` perpendicular to the pointing axis is then: + +.. math:: + + {\bf v}_{\perp} = \hat{\bf p}_B \times \hat{\bf v}_{LP/R} + +and is normalized to :math:`\hat{\bf v}_{\perp}`. + +If :math:`|\hat{\bf p}_B \times \hat{\bf v}_{LP/R}|` is smaller than a user-defined threshold ``alignmentThreshold`` +(default 0.1), then :math:`\hat{\bf p}_B` and :math:`\hat{\bf v}_{LP/R}` are nearly collinear and the perpendicular +direction is undefined or unstable. In that case no extra rotation is applied and the outputs +fall back to the standard location-pointing values. + +**Step 2 – Choose the sign of** :math:`\hat{\bf v}_{\perp}`. +Both :math:`+\hat{\bf v}_{\perp}` and :math:`-\hat{\bf v}_{\perp}` are valid perpendicular-to-strip directions. +The sign is chosen so that the rotation angle is minimized, i.e. the sign that satisfies +:math:`\hat{\bf c}_B \cdot \hat{\bf v}_{\perp} > 0` is selected. + +**Step 3 – Compute the extra rotation MRP** :math:`{\pmb\sigma}_{R_2/R}`. +The rotation that aligns :math:`\hat{\bf c}_B` with :math:`\hat{\bf v}_{\perp}` about :math:`\hat{\bf p}_B` is +computed analogously to the standard pointing rotation. The eigen-axis is: + +.. math:: + + \hat{\bf e}_{2} = \frac{\hat{\bf c}_B \times \hat{\bf v}_{\perp}}{|\hat{\bf c}_B \times \hat{\bf v}_{\perp}|} + +and the rotation angle is: + +.. math:: + + \alpha = \arccos\!\left(\hat{\bf c}_B \cdot \hat{\bf v}_{\perp}\right) + +The MRP of this extra rotation is then: + +.. math:: + + {\pmb\sigma}_{R_2/R} = \tan(\alpha / 4) \; \hat{\bf e}_{2} + +**Step 4 – Compose to get the final reference.** +The combined reference attitude that both points :math:`\hat{\bf p}_B` at the target **and** aligns +:math:`\hat{\bf c}_B` perpendicular to the strip scanning direction is: + +.. math:: + + {\pmb\sigma}_{R_2/N} = {\pmb\sigma}_{R/N} \oplus {\pmb\sigma}_{R_2/R} + +**Step 5 – Compute the final tracking error.** +The tracking error is recomputed with respect to the updated reference: + +.. math:: + + {\pmb\sigma}_{B/R_2} = {\pmb\sigma}_{B/N} \ominus {\pmb\sigma}_{R_2/N} + +where :math:`\ominus` denotes MRP subtraction. + +The outputs in strip imaging mode are therefore: + +.. math:: + + {\pmb\sigma}_{B/R}^{\text{out}} &= {\pmb\sigma}_{B/R_2} \\ + {\pmb\sigma}_{R/N}^{\text{out}} &= {\pmb\sigma}_{R_2/N} + + +Angular Rate Computation +^^^^^^^^^^^^^^^^^^^^^^^^ The tracking error rates :math:`{\pmb\omega}_{B/R}` are obtained through numerical differentiation of the MRP values. During the first module ``Update`` evaluation the numerical differencing is not possible and this value is thus set to zero. -Using the attitude navigation and guidance messages, this module also computes the reference information in -the form of ``attRefOutMsg``. This additional output message is useful when working with modules that need -a reference message and cannot accept a guidance message. - .. note:: The module checks for several conditions such as heading vectors being collinear, the MRP switching during the numerical differentiation, etc. - + In strip imaging mode, the non-collinearity constraint between :math:`\hat{\bf p}_B` and the + target velocity is also checked. When the two vectors are nearly parallel, the extra strip + rotation is skipped and the module falls back to pure location pointing. User Guide ---------- -The one required variable that must be set is ``pHat_B``. This is body-fixed unit vector which is to be -pointed at the desired inertial location. - -The user should only connect one location of interest input message, either ``locationInMsg``, ``celBodyInMsg`` or ``scTargetInMsg``. -Connecting both will result in a warning and the module defaults to using the ``locationInMsg`` information. - -This 2D attitude control module provides two output messages in the form of :ref:`attGuidMsgPayload` and :ref:`attRefMsgPayload`. -The first guidance message, describing body relative to reference tracking errors, -can be directly connected to an attitude control module. However, at times we need to have the -attitude reference message as the output to feed to :ref:`attTrackingError`. Here the ``B/R`` states are subtracted -from the ``B/N`` states to obtain the equivalent ``R/N`` states. - -The variable ``smallAngle`` defined the minimum angular separation where two vectors are considered colinear. -It is defaulted to zero, but can be set to any desired value in radians. +For **location pointing**, the one required variable that must be set is ``pHat_B``. This is body-fixed unit vector which is to be +pointed at the desired inertial location. The user should only connect one location of interest input message, either ``locationInMsg``, +``celBodyInMsg`` or ``scTargetInMsg``. Connecting multiple will result in a warning and the module defaults to using the ground location, +planet location or spacecraft location, in that order. By default this is a 2D attitude control module in attitude and a 2D rate control. In particular, the rates about the desired heading axis are not damped. By setting the module variable ``useBoresightRateDamping`` to 1, the body rates about about the desired heading angle are added to the rate tracking error yielding a 3D rate control implementation. +Note that ``useBoresightRateDamping`` is ignored during strip imaging, as strip imaging already performs +full 3D attitude control. + +For **strip imaging**, the user should connect ``locationstripInMsg`` (a :ref:`StripStateMsgPayload` from :ref:`stripLocation`) +and additionally set the ``cHat_B`` body-fixed vector. This vector must be perpendicular to ``pHat_B`` and +represents the instrument cross-track axis that needs to be aligned perpendicular to the strip scanning direction. +The variable ``alignmentThreshold`` (default 0.1) controls the collinearity threshold: if +:math:`|\hat{\bf p}_B \times \hat{\bf v}_{LP/R}|` is below this value the extra strip rotation is skipped. + +This module in both modes provides two output messages in the form of :ref:`attGuidMsgPayload` and :ref:`attRefMsgPayload`. +The first guidance message, describing body relative to reference tracking errors, +can be directly connected to an attitude control module. However, at times we need to have the +attitude reference message as the output to feed to :ref:`attTrackingError`. + +The variable ``smallAngle`` defines an angular threshold (in radians, default zero) below which a rotation is +set to zero. It applies separately to the pointing rotation and the strip rotation. This is to avoid numerical issues when the angles are very small. diff --git a/src/simulation/environment/stripLocation/_UnitTest/test_unitStripLocation.py b/src/simulation/environment/stripLocation/_UnitTest/test_unitStripLocation.py new file mode 100644 index 00000000000..ffa5d10c1e5 --- /dev/null +++ b/src/simulation/environment/stripLocation/_UnitTest/test_unitStripLocation.py @@ -0,0 +1,699 @@ +# ISC License +# +# Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +import inspect +import os + +import numpy as np +import pytest +from Basilisk import __path__ +from Basilisk.architecture import messaging +from Basilisk.simulation import stripLocation +from Basilisk.simulation import spacecraft +from Basilisk.utilities import RigidBodyKinematics as rbk +from Basilisk.utilities import SimulationBaseClass +from Basilisk.utilities import macros +from Basilisk.utilities import orbitalMotion +from Basilisk.utilities import simIncludeGravBody +from Basilisk.utilities import unitTestSupport + +filename = inspect.getframeinfo(inspect.currentframe()).filename +path = os.path.dirname(os.path.abspath(filename)) +bskName = 'Basilisk' +splitPath = path.split(bskName) +bskPath = __path__[0] + + +def test_range_degenerate_strip(show_plots): + """ + Tests stripLocation with a degenerate strip (start == end): + + 1. Computes range correctly by evaluating slantRange; + 2. Tests whether elevation is correctly evaluated; + 3. Tests whether range limits impact access; + 4. Tests whether multiple spacecraft are supported in parallel. + + This is the stripLocation equivalent of the groundLocation test_range test, + verifying that a strip with identical start and end points behaves + like a single ground location. + + :return: + """ + + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + simulationTime = macros.sec2nano(10.) + simulationTimeStep = macros.sec2nano(1.) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + # Create a strip target with start == end + stripTarget = stripLocation.StripLocation() + stripTarget.ModelTag = "stripTarget" + stripTarget.planetRadius = orbitalMotion.REQ_EARTH * 1000. + stripTarget.maximumRange = 100e3 # meters + stripTarget.minimumElevation = np.radians(80.) + stripTarget.specifyLocationStart(np.radians(0.), np.radians(0.), 0.) + stripTarget.specifyLocationEnd(np.radians(0.), np.radians(0.), 0.) + stripTarget.acquisition_speed = 0 # stationary + scSim.AddModelToTask(simTaskName, stripTarget) + + # Write out mock spacecraft position messages + sc1_message = messaging.SCStatesMsgPayload() + sc1_message.r_BN_N = [orbitalMotion.REQ_EARTH * 1e3 + 100e3, 0, 0] # SC1 directly overhead, in range + sc1Msg = messaging.SCStatesMsg().write(sc1_message) + + sc2_message = messaging.SCStatesMsgPayload() + sc2_message.r_BN_N = [orbitalMotion.REQ_EARTH * 1e3 + 101e3, 0, 0] # SC2 out of range + sc2Msg = messaging.SCStatesMsg().write(sc2_message) + + sc3_message = messaging.SCStatesMsgPayload() # SC3 is inside the altitude limit, but outside the visibility cone + sc3_message.r_BN_N = rbk.euler3(np.radians(11.)).dot(np.array([100e3, 0, 0])) + np.array( + [orbitalMotion.REQ_EARTH * 1e3, 0, 0]) + sc3Msg = messaging.SCStatesMsg().write(sc3_message) + + stripTarget.addSpacecraftToModel(sc1Msg) + stripTarget.addSpacecraftToModel(sc2Msg) + stripTarget.addSpacecraftToModel(sc3Msg) + + # Log the access indicator + numDataPoints = 2 + samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + dataLog0 = stripTarget.accessOutMsgs[0].recorder(samplingTime) + dataLog1 = stripTarget.accessOutMsgs[1].recorder(samplingTime) + dataLog2 = stripTarget.accessOutMsgs[2].recorder(samplingTime) + scSim.AddModelToTask(simTaskName, dataLog0) + scSim.AddModelToTask(simTaskName, dataLog1) + scSim.AddModelToTask(simTaskName, dataLog2) + + # Run the sim + scSim.InitializeSimulation() + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + # Get the logged data + sc1_access = dataLog0.hasAccess + sc1_slant = dataLog0.slantRange + sc1_elevation = dataLog0.elevation + + sc2_access = dataLog1.hasAccess + sc2_slant = dataLog1.slantRange + sc2_elevation = dataLog1.elevation + + sc3_access = dataLog2.hasAccess + sc3_slant = dataLog2.slantRange + sc3_elevation = dataLog2.elevation + + # Compare to expected values + accuracy = 1e-8 + ref_ranges = [100e3, 101e3, 100e3] + ref_elevation = [np.radians(90.), np.radians(90.), np.radians(79.)] + ref_access = [1, 0, 0] + + test_ranges = [sc1_slant[1], sc2_slant[1], sc3_slant[1]] + test_elevation = [sc1_elevation[1], sc2_elevation[1], sc3_elevation[1]] + test_access = [sc1_access[1], sc2_access[1], sc3_access[1]] + + range_worked = test_ranges == pytest.approx(ref_ranges, accuracy) + elevation_worked = test_elevation == pytest.approx(ref_elevation, accuracy) + access_worked = test_access == pytest.approx(ref_access, abs=1e-16) + + assert range_worked, f"Range check failed: {test_ranges} vs {ref_ranges}" + assert elevation_worked, f"Elevation check failed: {test_elevation} vs {ref_elevation}" + assert access_worked, f"Access check failed: {test_access} vs {ref_access}" + + +def test_rotation_degenerate_strip(show_plots): + """ + Tests whether stripLocation correctly accounts for planet rotation: + + 1. Computes the current strip location based on the initial position and the rotation + state of the planet it is attached to. + + A strip at (0, 10deg) with the planet rotated by -10deg around z should appear + at (0, 0) in inertial space, directly beneath a spacecraft on the x-axis. + + :return: + """ + simTime = 1. + + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + simulationTime = macros.sec2nano(simTime) + simulationTimeStep = macros.sec2nano(1.) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + # Create a degenerate strip at (0, 10deg) + stripTarget = stripLocation.StripLocation() + stripTarget.ModelTag = "stripTarget" + stripTarget.planetRadius = orbitalMotion.REQ_EARTH * 1000. + stripTarget.maximumRange = 200e3 # meters + stripTarget.minimumElevation = np.radians(10.) + stripTarget.specifyLocationStart(np.radians(0.), np.radians(10.), 0.) + stripTarget.specifyLocationEnd(np.radians(0.), np.radians(10.), 0.) + stripTarget.acquisition_speed = 0 + scSim.AddModelToTask(simTaskName, stripTarget) + + # Write out mock planet rotation and spacecraft position messages + sc1_message = messaging.SCStatesMsgPayload() + sc1_message.r_BN_N = np.array([orbitalMotion.REQ_EARTH * 1e3 + 90e3, 0, 0]) + scMsg = messaging.SCStatesMsg().write(sc1_message) + stripTarget.addSpacecraftToModel(scMsg) + + # Rotate planet by -10 deg so the (0, 10deg) ground point aligns with inertial x-axis + planet_message = messaging.SpicePlanetStateMsgPayload() + planet_message.J20002Pfix = rbk.euler3(np.radians(-10.)).tolist() + planetMsg = messaging.SpicePlanetStateMsg().write(planet_message) + stripTarget.planetInMsg.subscribeTo(planetMsg) + + # Log the access indicator + numDataPoints = 2 + samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + dataLog = stripTarget.accessOutMsgs[0].recorder(samplingTime) + scSim.AddModelToTask(simTaskName, dataLog) + + # Run the sim + scSim.InitializeSimulation() + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + # Get the logged data + sc1_access = dataLog.hasAccess + sc1_slant = dataLog.slantRange + sc1_elevation = dataLog.elevation + + # Compare to expected values + accuracy = 1e-8 + ref_ranges = [90e3] + ref_elevation = [np.radians(90.)] + ref_access = [1] + + test_ranges = [sc1_slant[1]] + test_elevation = [sc1_elevation[1]] + test_access = [sc1_access[1]] + + range_worked = test_ranges == pytest.approx(ref_ranges, accuracy) + elevation_worked = test_elevation == pytest.approx(ref_elevation, accuracy) + access_worked = test_access == pytest.approx(ref_access, abs=1e-16) + + assert range_worked, f"Range check failed: {test_ranges} vs {ref_ranges}" + assert elevation_worked, f"Elevation check failed: {test_elevation} vs {ref_elevation}" + assert access_worked, f"Access check failed: {test_access} vs {ref_access}" + + +def test_strip_target_motion(show_plots): + """ + Tests that the strip target position moves along the great-circle arc + from start to end as time progresses (No pre-imaging is considered in this test): + + 1. Verifies that the target position at the midpoint of the traversal + matches the expected SLERP interpolation on the sphere. + 2. Verifies that the target starts at the start point and finishes at the end point. + 3. Verifies that a spacecraft placed above the midpoint sees ~90deg elevation + when the target arrives there. + + :return: + """ + + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + dt = 1.0 + simulationTimeStep = macros.sec2nano(dt) + totalTimeSec = 10.0 + simulationTime = macros.sec2nano(totalTimeSec) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + R = orbitalMotion.REQ_EARTH * 1000. # planet radius in meters + + # Create a strip from (lat=0, lon=0) to (lat=0, lon=90deg) on the equator + stripTarget = stripLocation.StripLocation() + stripTarget.ModelTag = "stripTarget" + stripTarget.planetRadius = R + stripTarget.maximumRange = -1 # unlimited range + stripTarget.minimumElevation = np.radians(10.) + stripTarget.specifyLocationStart(np.radians(0.), np.radians(0.), 0.) + stripTarget.specifyLocationEnd(np.radians(0.), np.radians(90.), 0.) + + # Set acquisition speed so traversal takes exactly totalTimeSec seconds + strip_length = (np.pi / 2.0) * R # 90-degree arc length + acquisition_speed = strip_length / macros.sec2nano(totalTimeSec) # m/ns + stripTarget.acquisition_speed = acquisition_speed + stripTarget.pre_imaging_time = 0 + scSim.AddModelToTask(simTaskName, stripTarget) + + # Place SC above the midpoint (lon=45deg on the equator) + midpoint_dir = np.array([np.cos(np.radians(45.)), np.sin(np.radians(45.)), 0.]) + sc_altitude = 200e3 # meters + sc1_message = messaging.SCStatesMsgPayload() + sc1_message.r_BN_N = (R + sc_altitude) * midpoint_dir + sc1Msg = messaging.SCStatesMsg().write(sc1_message) + stripTarget.addSpacecraftToModel(sc1Msg) + + # Log strip state and access at every simulation step + stateLog = stripTarget.currentStripStateOutMsg.recorder(simulationTimeStep) + accessLog = stripTarget.accessOutMsgs[0].recorder(simulationTimeStep) + scSim.AddModelToTask(simTaskName, stateLog) + scSim.AddModelToTask(simTaskName, accessLog) + + # Run the sim + scSim.InitializeSimulation() + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + # --- Check target positions at key time points --- + r_LP_N_logged = stateLog.r_LP_N + pos_accuracy = 1.0 # 1 meter tolerance + + # At t=0s (index 0): target should be at start (lon=0) -> PCPF = [R, 0, 0] + expected_start = np.array([R, 0., 0.]) + start_error = np.linalg.norm(r_LP_N_logged[0] - expected_start) + assert start_error < pos_accuracy, \ + f"Target position error at start: {start_error:.6f} m (expected < {pos_accuracy} m)" + + # At t=5s (index 5): target should be at midpoint (lon=45) -> [R/sqrt(2), R/sqrt(2), 0] + expected_mid = R * midpoint_dir + mid_error = np.linalg.norm(r_LP_N_logged[5] - expected_mid) + assert mid_error < pos_accuracy, \ + f"Target position error at midpoint: {mid_error:.6f} m (expected < {pos_accuracy} m)" + + # At t=10s (index 10): target should be at end (lon=90) -> [0, R, 0] + expected_end = np.array([0., R, 0.]) + end_error = np.linalg.norm(r_LP_N_logged[10] - expected_end) + assert end_error < pos_accuracy, \ + f"Target position error at end: {end_error:.6f} m (expected < {pos_accuracy} m)" + + # At midpoint, SC should be directly overhead -> elevation ~ 90 deg + elevation_at_midpoint = accessLog.elevation[5] + assert elevation_at_midpoint == pytest.approx(np.radians(90.), abs=0.01), \ + f"Elevation at midpoint should be ~90 deg, got {np.degrees(elevation_at_midpoint):.2f} deg" + + # SC above the midpoint should have access when the target is at the midpoint + assert accessLog.hasAccess[5] == 1, "SC should have access when target is at midpoint" + + +def test_pre_imaging_blocks_access(show_plots): + """ + Tests that access is blocked during the pre-imaging phase: + + 1. A strip target with pre_imaging_time > 0 should deny access + during the initial pre-imaging period, even if the spacecraft + is geometrically visible. + 2. After the pre-imaging time has elapsed, access should be granted + if the elevation/range criteria are met. + + :return: + """ + + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + dt = 1.0 + simulationTimeStep = macros.sec2nano(dt) + totalTimeSec = 10.0 + simulationTime = macros.sec2nano(totalTimeSec) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + R = orbitalMotion.REQ_EARTH * 1000. + + # Create a stationary strip (start == end) with non-zero pre_imaging_time + stripTarget = stripLocation.StripLocation() + stripTarget.ModelTag = "stripTarget" + stripTarget.planetRadius = R + stripTarget.maximumRange = -1 # unlimited + stripTarget.minimumElevation = np.radians(10.) + stripTarget.specifyLocationStart(np.radians(0.), np.radians(0.), 0.) + stripTarget.specifyLocationEnd(np.radians(0.), np.radians(0.), 0.) + stripTarget.acquisition_speed = 0 # stationary + stripTarget.pre_imaging_time = 5.0e9 # 5 seconds in nanoseconds + scSim.AddModelToTask(simTaskName, stripTarget) + + # Place SC directly overhead + sc1_message = messaging.SCStatesMsgPayload() + sc1_message.r_BN_N = [R + 100e3, 0, 0] + sc1Msg = messaging.SCStatesMsg().write(sc1_message) + stripTarget.addSpacecraftToModel(sc1Msg) + + # Log access at every step + accessLog = stripTarget.accessOutMsgs[0].recorder(simulationTimeStep) + scSim.AddModelToTask(simTaskName, accessLog) + + # Run the sim + scSim.InitializeSimulation() + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + access = accessLog.hasAccess + + # During pre-imaging (t=0s to t=4s): duration < pre_imaging_time -> no access + # At t=0: duration_strip_imaging = 0 < 5e9 -> access = 0 + # At t=4: duration_strip_imaging = 4e9 < 5e9 -> access = 0 + for i in range(5): # indices 0..4 correspond to t=0s..t=4s + assert access[i] == 0, \ + f"Access should be blocked at t={i}s (pre-imaging), got {access[i]}" + + # After pre-imaging (t >= 5s): duration >= pre_imaging_time -> access granted + # At t=5: duration_strip_imaging = 5e9 >= 5e9 -> access = 1 + for i in range(5, 11): # indices 5..10 correspond to t=5s..t=10s + assert access[i] == 1, \ + f"Access should be granted at t={i}s (post pre-imaging), got {access[i]}" + + +def test_strip_velocity_output(show_plots): + """ + Tests that the strip state output message correctly reports the + velocity of the moving target along the strip: + + 1. For a non-degenerate strip, velocity magnitude should equal the acquisition speed. + 2. The velocity vector should be perpendicular to the position vector + (tangent to the sphere). + 3. The velocity vector should lie in the great-circle plane defined by the + strip start and end points (i.e. perpendicular to the orbit normal). + 4. The velocity vector should be oriented in the direction of motion + (from start toward end), verified by checking that r x v is parallel + to the orbit normal. + + :return: + """ + + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + dt = 1.0 + simulationTimeStep = macros.sec2nano(dt) + totalTimeSec = 10.0 + simulationTime = macros.sec2nano(totalTimeSec) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + R = orbitalMotion.REQ_EARTH * 1000. + + # Create a moving strip from (lat=0, lon=0) to (lat=0, lon=90) + stripTarget = stripLocation.StripLocation() + stripTarget.ModelTag = "stripTarget" + stripTarget.planetRadius = R + stripTarget.maximumRange = -1 + stripTarget.minimumElevation = np.radians(10.) + stripTarget.specifyLocationStart(np.radians(0.), np.radians(0.), 0.) + stripTarget.specifyLocationEnd(np.radians(0.), np.radians(90.), 0.) + + strip_length = (np.pi / 2.0) * R + acquisition_speed = strip_length / macros.sec2nano(totalTimeSec) # m/ns + stripTarget.acquisition_speed = acquisition_speed + stripTarget.pre_imaging_time = 0 + scSim.AddModelToTask(simTaskName, stripTarget) + + # Need at least one spacecraft for the module to function + sc1_message = messaging.SCStatesMsgPayload() + sc1_message.r_BN_N = [R + 200e3, 0, 0] + sc1Msg = messaging.SCStatesMsg().write(sc1_message) + stripTarget.addSpacecraftToModel(sc1Msg) + + # Log the strip state + stateLog = stripTarget.currentStripStateOutMsg.recorder(simulationTimeStep) + scSim.AddModelToTask(simTaskName, stateLog) + + # Run the sim + scSim.InitializeSimulation() + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + v_LP_N_logged = stateLog.v_LP_N + r_LP_N_logged = stateLog.r_LP_N + + # Compute the orbit-plane normal from the strip endpoints + # Start: (lat=0, lon=0) -> R * [1, 0, 0] + # End: (lat=0, lon=90) -> R * [0, 1, 0] + r_start = R * np.array([1.0, 0.0, 0.0]) + r_end = R * np.array([0.0, 1.0, 0.0]) + orbit_normal = np.cross(r_start, r_end) + orbit_normal = orbit_normal / np.linalg.norm(orbit_normal) # [0, 0, 1] + + # Check at several time indices (skip index 0 which is before motion starts) + for idx in [1, 3, 5, 7, 9]: + v = v_LP_N_logged[idx] + r = r_LP_N_logged[idx] + v_mag = np.linalg.norm(v) + r_mag = np.linalg.norm(r) + + # 1. Velocity magnitude should equal acquisition_speed (in m/ns) + assert v_mag == pytest.approx(acquisition_speed, rel=1e-6), \ + f"[idx={idx}] Velocity magnitude {v_mag} doesn't match acquisition speed {acquisition_speed}" + + # 2. Velocity should be perpendicular to the position vector + dot_rv = np.dot(r, v) / (r_mag * v_mag) + assert abs(dot_rv) < 1e-6, \ + f"[idx={idx}] Velocity should be perpendicular to position, dot product = {dot_rv}" + + # 3. Velocity should lie in the great-circle (orbit) plane: + # v . n_orbit ≈ 0 + dot_vn = np.dot(v, orbit_normal) / v_mag + assert abs(dot_vn) < 1e-6, \ + f"[idx={idx}] Velocity should lie in the orbit plane, v · n = {dot_vn}" + + # 4. Velocity should be oriented in the direction of motion: + # r x v should be parallel to orbit_normal (same sign) + rxv = np.cross(r, v) + rxv_hat = rxv / np.linalg.norm(rxv) + dot_direction = np.dot(rxv_hat, orbit_normal) + assert dot_direction > 0.99, \ + f"[idx={idx}] Velocity should point in direction of motion (start->end), " \ + f"(r x v) · n = {dot_direction}" + + +def test_new_start_pre_imaging(show_plots): + """ + Tests that ``newpstart()`` correctly extends the strip backward so that + the target reaches the *original* start point after exactly + ``pre_imaging_time`` seconds of travel: + + 1. Verifies that at t=0 the target position equals the extended + (new) start point, which sits ``pre_imaging_time * speed`` + arc-length behind the original start along the great circle. + 2. Verifies that at t=pre_imaging_time the target arrives at the + original start point (lat=0, lon=0). + 3. Verifies that at the end of the updated traversal the target + reaches the original end point (lat=0, lon=90°). + + :return: + """ + + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + dt = 1.0 + simulationTimeStep = macros.sec2nano(dt) + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + R = orbitalMotion.REQ_EARTH * 1000. # planet radius in metres + + # ---- strip geometry ---- + # Original strip along the equator from lon=0 to lon=90 deg + lat_start, lon_start = 0., 0. + lat_end, lon_end = 0., 90. + theta_orig = np.radians(lon_end - lon_start) # 90 deg arc + strip_length_orig = theta_orig * R # original arc length + + # Speeds chosen so the *original* strip takes 10 s to traverse + totalOriginalTimeSec = 10.0 + acquisition_speed = strip_length_orig / macros.sec2nano(totalOriginalTimeSec) # m / ns + + # Pre-imaging time = 2 s ⇒ target is extended 2 s of travel behind start + pre_imaging_sec = 2.0 + pre_imaging_ns = macros.sec2nano(pre_imaging_sec) + + # Expected arc-length behind the original start + arc_behind = acquisition_speed * pre_imaging_ns # metres + angle_behind = arc_behind / R # radians (≈ 18 deg) + + # Expected new start position: lon = -angle_behind on the equator + expected_new_start = R * np.array([np.cos(-angle_behind), + np.sin(-angle_behind), + 0.]) + + # Expected time to traverse the *updated* strip + # Updated arc covers (theta_orig + angle_behind) radians + theta_updated = theta_orig + angle_behind + totalUpdatedTimeSec = (theta_updated * R) / (acquisition_speed * 1e9) # seconds + + # Run sim long enough to cover the extended strip + simulationTime = macros.sec2nano(np.ceil(totalUpdatedTimeSec)) + + # ---- module setup ---- + stripTarget = stripLocation.StripLocation() + stripTarget.ModelTag = "stripTargetNewStart" + stripTarget.planetRadius = R + stripTarget.maximumRange = -1 + stripTarget.minimumElevation = np.radians(10.) + stripTarget.specifyLocationStart(np.radians(lat_start), np.radians(lon_start), 0.) + stripTarget.specifyLocationEnd(np.radians(lat_end), np.radians(lon_end), 0.) + stripTarget.acquisition_speed = acquisition_speed + stripTarget.pre_imaging_time = pre_imaging_ns + scSim.AddModelToTask(simTaskName, stripTarget) + + # A spacecraft is needed to drive the module (placed arbitrarily) + sc_msg = messaging.SCStatesMsgPayload() + sc_msg.r_BN_N = [(R + 200e3), 0, 0] + scMsg = messaging.SCStatesMsg().write(sc_msg) + stripTarget.addSpacecraftToModel(scMsg) + + # Log strip state at every step + stateLog = stripTarget.currentStripStateOutMsg.recorder(simulationTimeStep) + scSim.AddModelToTask(simTaskName, stateLog) + + # ---- run ---- + scSim.InitializeSimulation() + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + # ---- assertions ---- + r_LP_N_logged = stateLog.r_LP_N + pos_tol = 1.0 # 1 metre tolerance + + # 1) At t = 0 s (index 0): target should be at the *new* start (lon ≈ -18 deg) + new_start_error = np.linalg.norm(r_LP_N_logged[0] - expected_new_start) + assert new_start_error < pos_tol, \ + f"New start position error: {new_start_error:.4f} m (tol {pos_tol} m). " \ + f"Expected lon ≈ {np.degrees(-angle_behind):.2f} deg" + + # 2) At t = pre_imaging_sec (index = pre_imaging_sec / dt): target should be + # at the *original* start (lat=0, lon=0) -> PCPF [R, 0, 0] + idx_orig_start = int(pre_imaging_sec / dt) + expected_orig_start = np.array([R, 0., 0.]) + orig_start_error = np.linalg.norm(r_LP_N_logged[idx_orig_start] - expected_orig_start) + assert orig_start_error < pos_tol, \ + f"Target should be at original start at t={pre_imaging_sec}s, " \ + f"error = {orig_start_error:.4f} m (tol {pos_tol} m)" + + # 3) At the final step the target should be at the original end (lat=0, lon=90 deg) + # -> PCPF [0, R, 0] + idx_end = int(np.ceil(totalUpdatedTimeSec / dt)) + expected_end = np.array([0., R, 0.]) + end_error = np.linalg.norm(r_LP_N_logged[idx_end] - expected_end) + assert end_error < pos_tol, \ + f"Target should be at end at t={totalUpdatedTimeSec:.1f}s, " \ + f"error = {end_error:.4f} m (tol {pos_tol} m)" + + # 4) Verify the angular offset between new start and original start ≈ angle_behind + new_start_pos = r_LP_N_logged[0] + cos_angle_actual = np.dot(new_start_pos, expected_orig_start) / ( + np.linalg.norm(new_start_pos) * np.linalg.norm(expected_orig_start)) + angle_actual = np.arccos(np.clip(cos_angle_actual, -1.0, 1.0)) + assert angle_actual == pytest.approx(angle_behind, abs=1e-6), \ + f"Angle between new start and original start: {np.degrees(angle_actual):.4f} deg, " \ + f"expected {np.degrees(angle_behind):.4f} deg" + + +def test_AzElR_rates(): + """ + Tests that the Az, El, range rates are correct by using 1-step Euler integration. + Uses a stationary strip (start == end at Boulder, CO) to test rate computation, + analogous to the groundLocation Az/El/R rate test. + + :return: + """ + simTaskName = "simTask" + simProcessName = "simProcess" + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess(simProcessName) + dt = 1.0 + simulationTimeStep = macros.sec2nano(dt) + simulationTime = simulationTimeStep + dynProcess.addTask(scSim.CreateNewTask(simTaskName, simulationTimeStep)) + + gravFactory = simIncludeGravBody.gravBodyFactory() + planet = gravFactory.createEarth() + mu = planet.mu + planet.isCentralBody = True + timeInitString = '2021 MAY 04 06:47:48.965 (UTC)' + gravFactory.createSpiceInterface(bskPath + '/supportData/EphemerisData/', + timeInitString) + gravFactory.spiceObject.zeroBase = 'Earth' + scSim.AddModelToTask(simTaskName, gravFactory.spiceObject, -1) + + scObject = spacecraft.Spacecraft() + scObject.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) + scSim.AddModelToTask(simTaskName, scObject) + oe = orbitalMotion.ClassicElements() + r_sc = planet.radEquator + 100 * 1E3 + oe.a = r_sc + oe.e = 0.00001 + oe.i = 53.0 * macros.D2R + oe.Omega = 115.0 * macros.D2R + oe.omega = 5.0 * macros.D2R + oe.f = 75. * macros.D2R + rN, vN = orbitalMotion.elem2rv(mu, oe) + scObject.hub.r_CN_NInit = rN + scObject.hub.v_CN_NInit = vN + + # Use a stationary strip (start == end) at Boulder, CO + stripStation = stripLocation.StripLocation() + stripStation.planetRadius = planet.radEquator + stripStation.specifyLocationStart(np.radians(40.009971), np.radians(-105.243895), 1624) + stripStation.specifyLocationEnd(np.radians(40.009971), np.radians(-105.243895), 1624) + stripStation.acquisition_speed = 0 + stripStation.planetInMsg.subscribeTo(gravFactory.spiceObject.planetStateOutMsgs[0]) + stripStation.minimumElevation = np.radians(60.) + stripStation.addSpacecraftToModel(scObject.scStateOutMsg) + scSim.AddModelToTask(simTaskName, stripStation) + + # Log the Az, El, R and rates info + numDataPoints = 2 + samplingTime = unitTestSupport.samplingTime(simulationTime, simulationTimeStep, numDataPoints) + dataLog = stripStation.accessOutMsgs[0].recorder(samplingTime) + scSim.AddModelToTask(simTaskName, dataLog) + + # Run the sim + scSim.InitializeSimulation() + scSim.ConfigureStopTime(simulationTime) + scSim.ExecuteSimulation() + + # Get logged data + sc_range = dataLog.slantRange + sc_elevation = dataLog.elevation + sc_azimuth = dataLog.azimuth + sc_range_rate = dataLog.range_dot + sc_el_rate = dataLog.el_dot + sc_az_rate = dataLog.az_dot + + # Euler integration: value(t+dt) ≈ value(t) + rate(t)*dt + sc_Euler_range = sc_range[0] + sc_range_rate[0] * dt + sc_Euler_elev = sc_elevation[0] + sc_el_rate[0] * dt + sc_Euler_azimuth = sc_azimuth[0] + sc_az_rate[0] * dt + + range_rate_worked = sc_range[1] == pytest.approx(sc_Euler_range, rel=1e-5) + el_rate_worked = sc_elevation[1] == pytest.approx(sc_Euler_elev, rel=1e-5) + az_rate_worked = sc_azimuth[1] == pytest.approx(sc_Euler_azimuth, rel=1e-5) + + assert range_rate_worked, "Range rate check failed" + assert el_rate_worked, "Elevation rate check failed" + assert az_rate_worked, "Azimuth rate check failed" + + +if __name__ == '__main__': + test_range_degenerate_strip(False) + test_rotation_degenerate_strip(False) + test_strip_target_motion(False) + test_pre_imaging_blocks_access(False) + test_strip_velocity_output(False) + test_new_start_pre_imaging(False) + test_AzElR_rates() diff --git a/src/simulation/environment/stripLocation/stripLocation.cpp b/src/simulation/environment/stripLocation/stripLocation.cpp new file mode 100755 index 00000000000..cb88be1f0a1 --- /dev/null +++ b/src/simulation/environment/stripLocation/stripLocation.cpp @@ -0,0 +1,363 @@ +/* + ISC License + + Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "simulation/environment/stripLocation/stripLocation.h" +#include "architecture/utilities/avsEigenSupport.h" +#include "architecture/utilities/linearAlgebra.h" +#include +#include + +/*! @brief Creates an instance of the Strip Location Class + */ +StripLocation::StripLocation() +{ //! - Set some default initial conditions: + this->minimumElevation = 10.*D2R; // [rad] Minimum elevation above the target point; defaults to 10 degrees. + this->maximumRange = -1; // [m] Maximum range with the target point; defaults no maximum range (-1). + + this->currentStripStateBuffer = this->currentStripStateOutMsg.zeroMsgPayload; + + this->planetRadius = REQ_EARTH*1e3; + this->planetState = this->planetInMsg.zeroMsgPayload; + this->planetState.J20002Pfix[0][0] = 1; + this->planetState.J20002Pfix[1][1] = 1; + this->planetState.J20002Pfix[2][2] = 1; + this->r_North_N << 0, 0, 1; + + this->r_LP_P_Start.fill(0.0); // [m] Starting point of the strip relative to PCPF. + this->r_LP_P_End.fill(0.0); // [m] Ending point of the strip relative to PCPF. + + this->OldSimNanos = 0; // [ns] Currrent time when the simulation starts, used to compute the time already spent to image the strip. + + this->acquisition_speed = 3*1e-6; // [m/ns] Required acquisition speed. + + this->pre_imaging_time = 0; // [ns] Pre_imaging time used to artificially modify r_LP_P_Start. + this->isStartPositionUpdated = false; // [-] Flag indicating if the starting point has already been updated. +} + +/*! Empty destructor method. + */ +StripLocation::~StripLocation() +{ + for (long unsigned int c=0; caccessOutMsgs.size(); c++) { + delete this->accessOutMsgs.at(c); + } + return; +} + +/*! Resets the internal position to the specified initial position.*/ +void StripLocation::Reset(uint64_t CurrentSimNanos) +{ + this->isStartPositionUpdated = false; + this->duration_strip_imaging = 0; + this->OldSimNanos = CurrentSimNanos; + + if (this->planetRadius < 0) { + bskLogger.bskLog(BSK_ERROR, "StripLocation module must have planetRadius set."); + } +} + +/*! Specifies the start location of the strip from planet-centered latitude, longitude, altitude position + * + * @param lat + * @param longitude + * @param alt + */ +void StripLocation::specifyLocationStart(double lat, double longitude, double alt) +{ + Eigen::Vector3d tmpLLAPosition(lat, longitude, alt); + this->r_LP_P_Start = LLA2PCPF(tmpLLAPosition, this->planetRadius); +} + +/*! Specifies the end location of the strip from planet-centered latitude, longitude, altitude position + * + * @param lat + * @param longitude + * @param alt + */ +void StripLocation::specifyLocationEnd(double lat, double longitude, double alt) +{ + Eigen::Vector3d tmpLLAPosition(lat, longitude, alt); + this->r_LP_P_End = LLA2PCPF(tmpLLAPosition, this->planetRadius); +} + +/*! Updates the starting point of the strip to take into account the specified pre-imaging time + */ +void StripLocation::newpstart() +{ + this->p_start = this->r_LP_P_Start.normalized() * this->planetRadius; //[m] Making sure the location of the starting point is on the Earth surface for the interpolation (Assuming Spherical Earth). + this->p_end = this->r_LP_P_End.normalized() * this->planetRadius; //[m] Making sure the location of the ending point is on the Earth surface for the interpolation (Assuming Spherical Earth). + double dotVal = p_start.dot(p_end) / (this->planetRadius * this->planetRadius); + dotVal = std::max(-1.0, std::min(1.0, dotVal)); + this->theta = std::acos(dotVal); // [rad] + + // If the acquisition speed is equal to zero, if the pre_imaging_time is inferior or equal to zero, no pre-imaging time is applied. + if (this->acquisition_speed <= 0.0 || this->pre_imaging_time <= 0) { + this->p_start_updated = this->p_start; + this->isStartPositionUpdated = true; + return; + } + + this->lenght_central_line=this->theta* this->planetRadius; // [m] Lenght of the central line of the strip. + double line_speed_ratio = this->lenght_central_line / this->acquisition_speed; + double t= this->pre_imaging_time*-1 / line_speed_ratio; + + this->p_start_updated = this->PositionCentralLine(t, this->theta, this->p_start, this->p_end); //[m] Updated starting point of the strip relative to PCPF, taking into account the pre-imaging time (Spherical Extrapolation). + this->isStartPositionUpdated = true; +} + +/*! Adds a scState message name to the vector of names to be subscribed to. Also creates a corresponding access message output name. +*/ +void StripLocation::addSpacecraftToModel(Message *tmpScMsg) +{ + this->scStateInMsgs.push_back(tmpScMsg->addSubscriber()); + + /* create output message */ + Message *msg; + msg = new Message; + this->accessOutMsgs.push_back(msg); + + /* expand the buffer vector */ + AccessMsgPayload accMsg; + this->accessMsgBuffer.push_back(accMsg); +} + +/*! Read module messages +*/ +bool StripLocation::ReadMessages() +{ + SCStatesMsgPayload scMsg; + + /* clear out the vector of spacecraft states. This is created freshly below. */ + this->scStatesBuffer.clear(); + + //! - read in the spacecraft state messages + bool scRead; + if(!this->scStateInMsgs.empty()) + { + scRead = true; + for (long unsigned int c = 0; c < this->scStateInMsgs.size(); c++) { + scMsg = this->scStateInMsgs.at(c)(); + scRead = scRead && this->scStateInMsgs.at(c).isWritten(); + this->scStatesBuffer.push_back(scMsg); + } + } else { + bskLogger.bskLog(BSK_ERROR, "Ground location has no spacecraft to track."); + scRead = false; + } + //! - Read in the optional planet message. if no planet message is set, then a zero planet position, velocity and orientation is assumed + bool planetRead = true; + if(this->planetInMsg.isLinked()) + { + planetRead = this->planetInMsg.isWritten(); + this->planetState = this->planetInMsg(); + } + + return(planetRead && scRead); +} + +/*! write module messages +*/ +void StripLocation::WriteMessages(uint64_t CurrentClock) +{ + //! - write access message for each spacecraft + for (long unsigned int c=0; c< this->accessMsgBuffer.size(); c++) { + this->accessOutMsgs.at(c)->write(&this->accessMsgBuffer.at(c), this->moduleID, CurrentClock); + } + this->currentStripStateOutMsg.write(&this->currentStripStateBuffer, this->moduleID, CurrentClock); +} + +/*! Inertial position and velocity of the target point on the strip +*/ +void StripLocation::updateInertialPosition() +{ + // Rotation matrix from the inertial to planet frame from SPICE: + this->dcm_PN = cArray2EigenMatrix3d(*this->planetState.J20002Pfix); + this->dcm_PN_dot = cArray2EigenMatrix3d(*this->planetState.J20002Pfix_dot); + this->r_PN_N = cArray2EigenVector3d(this->planetState.PositionVector); + + // Position of the target in the inertial frame + this->r_LP_N = this->dcm_PN.transpose() * this->r_LP_P; + this->rhat_LP_N = this->r_LP_N/this->r_LP_N.norm(); + this->r_LN_N = this->r_PN_N + this->r_LP_N; + + // Get planet frame angular velocity vector + Eigen::Matrix3d w_tilde_PN = - this->dcm_PN_dot * this->dcm_PN.transpose(); + this->w_PN << w_tilde_PN(2,1), w_tilde_PN(0,2), w_tilde_PN(1,0); + + // Transform the velocity to the inertial frame + // Include planet rotation to obtain inertial target-track velocity. + this->v_LP_N = this->dcm_PN.transpose() * (this->v_LP_P + this->w_PN.cross(this->r_LP_P)); + + // Compute full inertial velocity: v_LN_N = v_PN_N + v_LP_N + Eigen::Vector3d v_PN_N = cArray2EigenVector3d(this->planetState.VelocityVector); + this->v_LN_N = v_PN_N + this->v_LP_N; + + // Stash updated position in the StripState message + eigenVector3d2CArray(this->r_LN_N, this->currentStripStateBuffer.r_LN_N); + eigenVector3d2CArray(this->r_LP_N, this->currentStripStateBuffer.r_LP_N); + + // Stash updated velocity in the StripState message + eigenVector3d2CArray(this->v_LP_N, this->currentStripStateBuffer.v_LP_N); + eigenVector3d2CArray(this->v_LN_N, this->currentStripStateBuffer.v_LN_N); +} + +/*! Returns target point on the central line of the strip relative to PCPF */ +Eigen::Vector3d StripLocation::PositionCentralLine(double t, double teta, const Eigen::Vector3d& p_start, const Eigen::Vector3d& p_end) const { + // If points are too close (theta ~ 0), return p_start to avoid division by zero + if (std::abs(teta) < 1e-12) { + return p_start; + } + // Perform spherical linear interpolation + double sinTheta = std::sin(teta); + double coeff1 = std::sin((1 - t) * teta) / sinTheta; + double coeff2 = std::sin(t * teta) / sinTheta; + Eigen::Vector3d interpolatedPoint = coeff1 * p_start + coeff2 * p_end; + + // Return the interpolated point on the Earth's surface + return interpolatedPoint.normalized() * this->planetRadius; +} + +/*! Returns the tangent velocity vector to the central line path of the target point relative to PCPF */ +Eigen::Vector3d StripLocation::VelocityCentralLine(double t, double teta, const Eigen::Vector3d& p_start, const Eigen::Vector3d& p_end) const { + // If points are too close (theta ~ 0), return zero velocity to avoid division by zero + if (std::abs(teta) < 1e-12) { + return Eigen::Vector3d::Zero(); + } + + double sinTheta = std::sin(teta); + double coeff1 = std::cos((1 - t) * teta) * (-teta) / sinTheta; + double coeff2 = std::cos(t * teta) * teta / sinTheta; + + Eigen::Vector3d velocity = coeff1 * p_start + coeff2 * p_end; + double velocityNorm = velocity.norm(); // Not zero thanks to the check of teta + return velocity / velocityNorm * this->acquisition_speed; +} + +/*! Update the target point on the central line */ +void StripLocation::updateTargetPositionPCPF(uint64_t CurrentClock) + { + if (!this->isStartPositionUpdated) { + this->newpstart(); + } + + double dotVal = p_start_updated.dot(p_end) / (this->planetRadius * this->planetRadius); + dotVal = std::max(-1.0, std::min(1.0, dotVal)); + this->teta_updated = std::acos(dotVal); // [rad] Angle between p_start_updated and p_end + + // Time already spent to image the strip + this->lenght_central_line_updated=this->teta_updated* this->planetRadius; + this->duration_strip_imaging = this->duration_strip_imaging + (CurrentClock-this->OldSimNanos); + + if (this->acquisition_speed <= 0.0) { + this->r_LP_P = this->p_start_updated; + this->v_LP_P.setZero(); + Eigen::Vector3d tmpLLAPosition = PCPF2LLA(this->r_LP_P, this->planetRadius); + this->dcm_LP = C_PCPF2SEZ(tmpLLAPosition[0], tmpLLAPosition[1]); + return; + } + + double line_speed_ratio = this->lenght_central_line_updated / this->acquisition_speed; + double tau = this->duration_strip_imaging / line_speed_ratio; + double tauClamped = std::max(0.0, std::min(1.0, tau)); + + //Update of the position vector of the target point on the central line an update the dcm_LP + this->r_LP_P = this->PositionCentralLine(tauClamped, this->teta_updated, this->p_start_updated, this->p_end); + Eigen::Vector3d tmpLLAPosition = PCPF2LLA(this->r_LP_P, this->planetRadius); + this->dcm_LP = C_PCPF2SEZ(tmpLLAPosition[0], tmpLLAPosition[1]); + + // Update the velocity vector + this->v_LP_P = this->VelocityCentralLine(tauClamped, this->teta_updated, this->p_start_updated, this->p_end); + + } + +void StripLocation::computeAccess() +{ + // Update the groundLocation's inertial position + this->updateInertialPosition(); + + // Iterate over spacecraft position messages and compute the access for each one + std::vector::iterator accessMsgIt; + std::vector::iterator scStatesMsgIt; + for(scStatesMsgIt = this->scStatesBuffer.begin(), accessMsgIt = accessMsgBuffer.begin(); scStatesMsgIt != scStatesBuffer.end(); scStatesMsgIt++, accessMsgIt++){ + //! Compute the relative position of each spacecraft to the site in the planet-centered inertial frame + Eigen::Vector3d r_BP_N = cArray2EigenVector3d(scStatesMsgIt->r_BN_N) - this->r_PN_N; + Eigen::Vector3d r_BL_N = r_BP_N - this->r_LP_N; + auto r_BL_mag = r_BL_N.norm(); + if (r_BL_mag < 1e-12) { + accessMsgIt->slantRange = 0.0; + accessMsgIt->elevation = -M_PI_2; + accessMsgIt->azimuth = 0.0; + accessMsgIt->range_dot = 0.0; + accessMsgIt->az_dot = 0.0; + accessMsgIt->el_dot = 0.0; + accessMsgIt->hasAccess = 0; + Eigen::Vector3d zeroVec = Eigen::Vector3d::Zero(); + eigenVector3d2CArray(zeroVec, accessMsgIt->r_BL_L); + eigenVector3d2CArray(zeroVec, accessMsgIt->v_BL_L); + continue; + } + Eigen::Vector3d relativeHeading_N = r_BL_N / r_BL_mag; + + double viewAngle = (M_PI_2-safeAcos(this->rhat_LP_N.dot(relativeHeading_N))); + + accessMsgIt->slantRange = r_BL_mag; + accessMsgIt->elevation = viewAngle; + Eigen::Vector3d r_BL_L = this->dcm_LP * this->dcm_PN * r_BL_N; + eigenVector3d2CArray(r_BL_L, accessMsgIt->r_BL_L); + double cos_az = -r_BL_L[0]/(sqrt(pow(r_BL_L[0],2) + pow(r_BL_L[1],2))); + double sin_az = r_BL_L[1]/(sqrt(pow(r_BL_L[0],2) + pow(r_BL_L[1],2))); + accessMsgIt->azimuth = atan2(sin_az, cos_az); + + Eigen::Vector3d v_BL_L = this->dcm_LP * this->dcm_PN * (cArray2EigenVector3d(scStatesMsgIt->v_BN_N) - this->w_PN.cross(r_BP_N)); // V observed from gL wrt P frame, expressed in L frame coords (SEZ) + eigenVector3d2CArray(v_BL_L, accessMsgIt->v_BL_L); + accessMsgIt->range_dot = v_BL_L.dot(r_BL_L)/r_BL_mag; + double xy_norm = sqrt(pow(r_BL_L[0],2)+pow(r_BL_L[1],2)); + if (xy_norm < 1e-12) { + accessMsgIt->azimuth = 0.0; + accessMsgIt->az_dot = 0.0; + accessMsgIt->el_dot = 0.0; + } else { + accessMsgIt->az_dot = (-r_BL_L[0]*v_BL_L[1] + r_BL_L[1]*v_BL_L[0])/pow(xy_norm,2); + accessMsgIt->el_dot = (v_BL_L[2]/xy_norm - r_BL_L[2]*(r_BL_L[0]*v_BL_L[0] + r_BL_L[1]*v_BL_L[1])/pow(xy_norm,3))/(1+pow(r_BL_L[2]/xy_norm,2)); + } + + if( (viewAngle > this->minimumElevation) && (r_BL_mag <= this->maximumRange || this->maximumRange < 0)&& (this->duration_strip_imaging >= this->pre_imaging_time)){ + accessMsgIt->hasAccess = 1; + } + else + { + accessMsgIt->hasAccess = 0; + } + } +} + +/*! + update module + @param CurrentSimNanos + */ +void StripLocation::UpdateState(uint64_t CurrentSimNanos) +{ + // Update the target point + this->ReadMessages(); + this->updateTargetPositionPCPF(CurrentSimNanos); + this->computeAccess(); + this->WriteMessages(CurrentSimNanos); + this->OldSimNanos = CurrentSimNanos; + +} diff --git a/src/simulation/environment/stripLocation/stripLocation.h b/src/simulation/environment/stripLocation/stripLocation.h new file mode 100644 index 00000000000..ac7ce7ac27f --- /dev/null +++ b/src/simulation/environment/stripLocation/stripLocation.h @@ -0,0 +1,105 @@ +/* + ISC License + + Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef STRIP_LOCATION_H +#define STRIP_LOCATION_H + +#include +#include +#include +#include "architecture/_GeneralModuleFiles/sys_model.h" + +#include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h" +#include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" +#include "architecture/msgPayloadDefC/AccessMsgPayload.h" +#include "architecture/msgPayloadDefC/StripStateMsgPayload.h" +#include "architecture/messaging/messaging.h" + +#include "architecture/utilities/geodeticConversion.h" +#include "architecture/utilities/astroConstants.h" +#include "architecture/utilities/bskLogging.h" + +/*! @brief strip location class */ +class StripLocation: public SysModel { +public: + StripLocation(); + ~StripLocation(); + void UpdateState(uint64_t CurrentSimNanos); + void Reset(uint64_t CurrentSimNanos); + bool ReadMessages(); + void WriteMessages(uint64_t CurrentClock); + void addSpacecraftToModel(Message *tmpScMsg); + void specifyLocationStart(double lat, double longitude, double alt); + void specifyLocationEnd(double lat, double longitude, double alt); + +private: + void updateInertialPosition(); + void updateTargetPositionPCPF(uint64_t CurrentClock); + void computeAccess(); + void newpstart(); + Eigen::Vector3d PositionCentralLine(double t, double teta, const Eigen::Vector3d& p_start, const Eigen::Vector3d& p_end) const; + Eigen::Vector3d VelocityCentralLine(double t, double teta, const Eigen::Vector3d& p_start, const Eigen::Vector3d& p_end) const; + + +public: + double minimumElevation; //!< [rad] minimum elevation above the local horizon needed to see a spacecraft; defaults to 10 degrees equivalent. + double maximumRange; //!< [m] (optional) Maximum slant range to compute access for; defaults to -1, which represents no maximum range. + double planetRadius; //!< [m] Planet radius in meters. + Eigen::Vector3d r_LP_P_Start; //!< [m] Ground location of the first point to image on the central line of the strip relative to PCPF + Eigen::Vector3d r_LP_P_End; //!< [m] Ground location of the last point to image on the central line of the strip relative to PCPF + Eigen::Vector3d p_start; //!< [m] Making sure the location of the first point is on the Earth surface for the interpolation + Eigen::Vector3d p_start_updated; //!< [m] Updated first point used by target propagation after pre-imaging start update + Eigen::Vector3d p_end; //!< [m] Making sure the location of the last point is on the Earth surface for the interpolation + double acquisition_speed; //!< [m/s] Constant acquisition speed of the camera + double pre_imaging_time; //!< [s] Pre_imaging time used to artificially modify r_LP_P_Start + ReadFunctor planetInMsg; //!< planet state input message + Message currentStripStateOutMsg; //!< strip location output message + std::vector*> accessOutMsgs; //!< vector of ground location access messages + std::vector> scStateInMsgs; //!< vector of sc state input messages + BSKLogger bskLogger; //!< -- BSK Logging + double theta;//!< [rad] Angle between r_LP_P_Start and r_LP_P_End from the center of Earth (The Earth is assumed perfectly spherical.) + double teta_updated; //!< [rad] Updated angle between p_start_updated and p_end used in target propagation + double lenght_central_line; //!< [m] Lenght of the central line + double lenght_central_line_updated; //!< [m] Updated length of the central line + Eigen::Vector3d r_PN_N; //!< [m] Planet position vector relative to inertial frame origin. + Eigen::Vector3d v_LP_N; //!< [m/s] Velocity vector of the current target point relative to the planet in the inertial frame + Eigen::Vector3d v_LN_N; //!< [m/s] Velocity vector of the current target point relative to the inertial origin in the inertial frame + uint64_t duration_strip_imaging; //!< [s] Time already spent to image the strip + uint64_t OldSimNanos; //!< [s] Previous CurrentSimNanos + +private: + StripStateMsgPayload currentStripStateBuffer; //!< buffer of ground station output data + SpicePlanetStateMsgPayload planetState; //!< buffer of planet data + Eigen::Vector3d r_North_N; //!<[-] Inertial 3rd axis, defined internally as "North". + bool isStartPositionUpdated; //!< [-] Flag indicating if the mutable start position has already been computed. + Eigen::Vector3d r_LP_P; //!< [m] Ground location of the current target point on the central line of the strip relative to PCPF + Eigen::Vector3d v_LP_P; //!< [m/s] Velocity of the current target point on the central line of the strip relative to PCPF + + + std::vector accessMsgBuffer; //!< buffer of access output data + std::vector scStatesBuffer; //!< buffer of spacecraft states + Eigen::Matrix3d dcm_LP; //!< Rotation matrix from planet-centered, planet-fixed frame P to site-local topographic (SEZ) frame L coordinates. + Eigen::Matrix3d dcm_PN; //!< Rotation matrix from inertial frame N to planet-centered to planet-fixed frame P. + Eigen::Matrix3d dcm_PN_dot; //!< Rotation matrix derivative from inertial frame N to planet-centered to planet-fixed frame P. + Eigen::Vector3d w_PN; //!< [rad/s] Angular velocity of planet-fixed frame P relative to inertial frame N. + Eigen::Vector3d r_LP_N; //!< [m] Gound Location position vector relative to planet origin vector in inertial coordinates. + Eigen::Vector3d rhat_LP_N;//!< [-] Surface normal vector from the target location in inertial coordinates. + Eigen::Vector3d r_LN_N;//!< [m] Ground Location position vector relative to inertial frame origin in inertial coordinates. +}; +#endif /* StripLocation */ diff --git a/src/simulation/environment/stripLocation/stripLocation.i b/src/simulation/environment/stripLocation/stripLocation.i new file mode 100644 index 00000000000..73e01a283ea --- /dev/null +++ b/src/simulation/environment/stripLocation/stripLocation.i @@ -0,0 +1,49 @@ +/* + ISC License + + Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%module stripLocation +%{ + #include "stripLocation.h" +%} + +%pythoncode %{ +from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_conly_data.i" +%include "swig_eigen.i" + +%include "sys_model.i" +%include "stripLocation.h" +%include "std_vector.i" + + +%include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h" +struct SpicePlanetStateMsg_C; +%include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" +struct SCStatesMsg_C; +%include "architecture/msgPayloadDefC/AccessMsgPayload.h" +struct AccessMsg_C; +%include "architecture/msgPayloadDefC/StripStateMsgPayload.h" +struct StripStateMsg_C; + + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} diff --git a/src/simulation/environment/stripLocation/stripLocation.rst b/src/simulation/environment/stripLocation/stripLocation.rst new file mode 100644 index 00000000000..8156f8fc4b6 --- /dev/null +++ b/src/simulation/environment/stripLocation/stripLocation.rst @@ -0,0 +1,202 @@ + +Executive Summary +----------------- +This module extends the :ref:`groundLocation` concept to model a **strip imaging target** on a +celestial body's surface. Instead of a single fixed point, the user defines a ground strip by +specifying a **start** and an **end** location (both body-fixed). The module uses spherical +linear interpolation (SLERP) to sweep a moving target point :math:`L` along the great-circle arc +connecting these two locations at a configurable constant acquisition speed. A **pre-imaging time** +parameter allows the strip's effective start point to be extrapolated backwards along the great-circle +arc so that the target point begins moving before the nominal start location, giving the spacecraft +time to slew. + +The inertial position and velocity of the current target point :math:`L` are evaluated and output as +a :ref:`StripStateMsgPayload` message. + +One or more spacecraft states can be added to compute each spacecraft's position relative +to the current target point :math:`L`. The associated :ref:`AccessMsgPayload` output message contains the +relative position vector in spherical coordinates (range, azimuth, elevation) and in +South-East-Zenith (SEZ) Cartesian coordinates, as well as the corresponding coordinate rates. +An integer flag indicates whether the spacecraft has access (i.e. is above the minimum +elevation angle of the :math:`L` SEZ frame, within range, and the pre-imaging time has elapsed). + +Module Assumptions and Limitations +---------------------------------- +- The body is assumed to be a **sphere** with a constant radius (``planetRadius``). +- The strip's central line is a great-circle arc on that sphere; local terrain is not considered. +- The target point moves along the arc at a **constant acquisition speed** (``acquisition_speed``). +- Elevation constraints are computed assuming a conical field of view around the surface normal + at the current target location. +- The pre-imaging time feature uses the same SLERP extrapolation, extending the arc backwards. + +Message Connection Descriptions +------------------------------- +The following table lists all the module input and output messages. The module msg variable name is set by the +user from python. The msg type contains a link to the message structure definition, while the description +provides information on what this message is used for. + +.. list-table:: Module I/O Messages + :widths: 25 25 50 + :header-rows: 1 + + * - Msg Variable Name + - Msg Type + - Description + * - planetInMsg + - :ref:`SpicePlanetStateMsgPayload` + - (optional) planet state input message. Default is a zero state for the planet. + * - scStateInMsgs + - :ref:`SCStatesMsgPayload` + - vector of sc state input messages. These are set through ``addSpacecraftToModel()`` + * - currentStripStateOutMsg + - :ref:`StripStateMsgPayload` + - strip location output message providing the current target point inertial position and velocity + * - accessOutMsgs + - :ref:`AccessMsgPayload` + - vector of access output messages (one per spacecraft) + + +Detailed Module Description +--------------------------- +The ``stripLocation`` module handles the following behavior: + +#. Reads in the planet's states using the ``planetInMsg`` input message. +#. Converts the start and end latitude/longitude/altitude coordinates to + planet-centered, planet-fixed (PCPF) coordinates. +#. (Optionally) Computes an updated start point by extrapolating backwards along the + great-circle arc using the ``pre_imaging_time`` parameter. +#. Propagates the current target point :math:`L` along the great-circle arc using SLERP, + at the constant ``acquisition_speed``. +#. Computes the inertial position and velocity of :math:`L`, accounting for planet rotation. +#. Converts each spacecraft's relative position to the SEZ frame and evaluates range, + azimuth, elevation, and their rates. +#. Determines spacecraft access (visibility) considering minimum elevation, maximum range, + and whether the pre-imaging time has elapsed. +#. Supports multiple spacecraft given one ``stripLocation`` instance. + + +Target Propagation along the Strip +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Given a start point :math:`\mathbf{p}_s` and end point :math:`\mathbf{p}_e` on the sphere of +radius :math:`R`, both expressed in PCPF coordinates, the central angle is: + +.. math:: \theta = \arccos\!\left(\frac{\mathbf{p}_s \cdot \mathbf{p}_e}{R^2}\right) + :label: eq:theta:1 + +The arc length of the central line is: + +.. math:: \ell = \theta \, R + :label: eq:arc_length:2 + +A normalised time parameter :math:`\tau \in [0,1]` is computed from the elapsed imaging time +:math:`\Delta t` and the acquisition speed :math:`v_a`: + +.. math:: \tau = \frac{\Delta t}{\ell / v_a} + :label: eq:tau:3 + +The target position is obtained via spherical linear interpolation (SLERP): + +.. math:: \mathbf{p}(\tau) = \frac{\sin\!\bigl((1-\tau)\,\theta\bigr)}{\sin\theta}\,\mathbf{p}_s + + \frac{\sin(\tau\,\theta)}{\sin\theta}\,\mathbf{p}_e + :label: eq:slerp:4 + +and projected back onto the sphere: + +.. math:: \mathbf{r}_{L/P}^P = R \, \frac{\mathbf{p}(\tau)}{\|\mathbf{p}(\tau)\|} + :label: eq:project:5 + +The PCPF velocity of the target is obtained by differentiating the SLERP expression and scaling +to the acquisition speed. + + +Determining Spacecraft-to-Target States +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +The position of the spacecraft relative to the current target point in the SEZ frame is +parameterized by the Cartesian coordinates: + +.. math:: \mathbf{r}_{B/L} = x\hat{S} + y\hat{E} + z\hat{Z} + :label: eq:strip_SEZ_coords:1 + +The Cartesian coordinates are converted to spherical coordinates centered at the target point, +giving the range (:math:`\rho`), azimuth (:math:`Az`), and elevation (:math:`El`): + +.. math:: \rho = \sqrt{x^2 + y^2 + z^2} + :label: eq:strip_rho:2 + +.. math:: Az = \arctan\!\frac{y}{x} + :label: eq:strip_az:3 + +.. math:: El = \arctan\!\frac{z}{\sqrt{x^2+y^2}} + :label: eq:strip_el:4 + +The spherical coordinate rates are computed by differentiating the range, azimuth, and elevation +with respect to the rotating SEZ frame: + +.. math:: \dot{\rho} = \frac{x\dot{x}+y\dot{y}+z\dot{z}}{\sqrt{x^2 + y^2 + z^2}} + :label: eq:strip_SEZ_rhoDot:5 + +.. math:: \dot{Az} = \frac{-x\dot{y}+y\dot{x}}{x^2+y^2} + :label: eq:strip_SEZ_AzDot:6 + +.. math:: \dot{El} = \frac{1}{1+\frac{z^2}{x^2+y^2}} \left( \frac{\dot{z}}{\sqrt{x^2+y^2}} - \frac{z(x\dot{x}+y\dot{y})}{(x^2+y^2)^{3/2}} \right) + :label: eq:strip_SEZ_ElDot:7 + + +User Guide +---------- + +To use this module, instantiate the class and provide it with two body-fixed locations (start and +end of the strip, in latitude/longitude/altitude) and the desired acquisition speed. A planet +position/attitude message (i.e., an instance of :ref:`SpicePlanetStateMsgPayload`) can optionally +be provided; to compute access, at least one :ref:`SCStatesMsgPayload` message must be added to +the module using the ``addSpacecraftToModel()`` method. The first spacecraft is index 0, the +second is 1, and so on. + +A new instance of ``stripLocation``, alongside necessary user-supplied parameters, can be created +by calling: + +.. code-block:: python + + stripTarget = stripLocation.StripLocation() + stripTarget.ModelTag = "stripTarget" + stripTarget.planetRadius = orbitalMotion.REQ_EARTH * 1000. # defaults to Earth's radius + stripTarget.maximumRange = 100e3 # (optional) maximum slant range for access [m] + stripTarget.minimumElevation = np.radians(10.) # minimum elevation for access [rad]; defaults to 10 deg + stripTarget.acquisition_speed = 3.0 # constant acquisition speed along the strip [m/s] + stripTarget.pre_imaging_time = 60.0 # (optional) pre-imaging offset time [s] + stripTarget.specifyLocationStart(np.radians(40.0), np.radians(-105.0), 0.) # start lat, lon, alt + stripTarget.specifyLocationEnd(np.radians(42.0), np.radians(-103.0), 0.) # end lat, lon, alt + scSim.AddModelToTask(simTaskName, stripTarget) + +The ``planetRadius`` variable is optional and defaults to Earth's radius. Strip endpoints are +specified through the ``specifyLocationStart()`` and ``specifyLocationEnd()`` methods, which +accept geodetic latitude, longitude (both in radians), and altitude (in meters). Alternatively, +the PCPF position vectors ``r_LP_P_Start`` and ``r_LP_P_End`` can be set directly. + +The ``maximumRange`` variable is optional and defaults to -1, meaning no maximum range is +considered. Set it to a positive value to have the ``hasAccess`` output depend on range. + +The ``pre_imaging_time`` variable is optional and defaults to 0. When set to a +positive value, the effective start point is extrapolated backwards along the great-circle arc +so that the target begins moving before the nominal start location, allowing time for +spacecraft maneuvers before the imaging pass begins. Access is not granted until the +pre-imaging phase has elapsed. + +A ``stripLocation`` can be affixed to a specific planet by setting its ``planetInMsg`` input +message: + +.. code-block:: python + + stripTarget.planetInMsg.subscribeTo(planetMsg) + +Spacecraft can be added to the model by calling: + +.. code-block:: python + + stripTarget.addSpacecraftToModel(sc1.scStateOutMsg) + stripTarget.addSpacecraftToModel(sc2.scStateOutMsg) + + # log code + dataLog0 = stripTarget.currentStripStateOutMsg.recorder() + dataLog1 = stripTarget.accessOutMsgs[0].recorder() + dataLog2 = stripTarget.accessOutMsgs[1].recorder() diff --git a/src/tests/test_scenarioStripImaging.py b/src/tests/test_scenarioStripImaging.py new file mode 100644 index 00000000000..d4b2699ea22 --- /dev/null +++ b/src/tests/test_scenarioStripImaging.py @@ -0,0 +1,81 @@ +# +# ISC License +# +# Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# + + +# +# Basilisk Scenario Script and Integrated Test +# +# Purpose: Integrated test of stripLocation and locationPointing for +# sequential strip imaging of three ground strips. +# Author: Autonomous Vehicle Systems Lab +# Creation Date: Mar. 12, 2026 +# + + +import inspect +import os +import sys + +import pytest +from Basilisk.utilities import unitTestSupport + +# Get current file path +filename = inspect.getframeinfo(inspect.currentframe()).filename +path = os.path.dirname(os.path.abspath(filename)) + +sys.path.append(path + '/../../examples') +import scenarioStripImaging + + +# uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed +# @pytest.mark.skipif(conditionstring) +# uncomment this line if this test has an expected failure, adjust message as needed +# @pytest.mark.xfail(True) + +# The following 'parametrize' function decorator provides the parameters and expected results for each +# of the multiple test runs for this test. +@pytest.mark.scenarioTest + +def test_scenarioStripImaging(show_plots): + """This function is called by the py.test environment.""" + + # provide a unique test method name, starting with test_ + # each test method requires a single assert method to be called + testFailCount = 0 # zero unit test result counter + testMessages = [] # create empty array to store test log messages + + try: + figureList = scenarioStripImaging.run(show_plots) + # save the figures to the Doxygen scenario images folder + for pltName, plt in list(figureList.items()): + unitTestSupport.saveScenarioFigure(pltName, plt, path) + + except OSError as err: + testFailCount += 1 + testMessages.append("scenarioStripImaging test has failed.") + + # print out success message if no error were found + if testFailCount == 0: + print("PASSED ") + else: + print(testFailCount) + print(testMessages) + + # each test method requires a single assert method to be called + # this check below just makes sure no sub-test failures were found + assert testFailCount < 1, testMessages