Add get_nsm_attitude() function from chandra_maneuver

ska_sun/sun.py

@@ -13,7 +13,7 @@
 from numpy import arcsin as asin
 from numpy import arctan2 as atan2
 from numpy import cos, degrees, pi, radians, sin
-from Quaternion import Quat
+from Quaternion import Quat, QuatLike
 from ska_helpers import chandra_models
 
 from ska_sun import conf
@@ -34,6 +34,7 @@
     "get_sun_pitch_yaw",
     "apply_sun_pitch_yaw",
     "get_att_for_sun_pitch_yaw",
+    "get_nsm_attitude",
 ]
 
 
@@ -794,3 +795,59 @@ def get_att_for_sun_pitch_yaw(
         coord_system=coord_system,
     )
     return att
+
+
+def get_nsm_attitude(att: QuatLike, time: CxoTimeLike, pitch: float = 90) -> Quat:
+    """
+    Calculate the closest Normal Sun Mode attitude from starting attitude.
+
+    The default is for a NSM pitch of 90 degrees (pure minus-Z at sun). An arbitrary
+    offset pitch angle can be specified.
+
+    The calculation is based on the Chandra - sun vector in ECI. The function defines
+    the vector in the body frame that will be pointed at the sun. The normal sun mode
+    maneuver is then calculated as the shortest maneuver that points that vector at the
+    Sun. Note that for off-nominal roll, this is NOT a pure pitch maneuver.
+
+    Parameters
+    ----------
+    att : Quat
+        Attitude that can initialize a Quat().
+    time : CxoTimeLike
+        Time in a valid Chandra.Time format.
+    pitch : float, optional
+        NSM pitch angle in degrees. The default is 90.
+
+    Returns
+    -------
+    Quat
+        NSM attitude quaternion.
+    """
+    # Calc Chanrda - sun vector in ECI (ignore CXO orbit)
+    (sun_ra, sun_dec) = position(time)
+    sun_eci = np.array(_radec2eci(sun_ra, sun_dec))
+
+    cxo_att = Quat(att)
+
+    # Define the vector in body frame that will be pointed at the sun.
+    # Pitch=90 (pure minus-Z at sun) : [0, 0, -1]
+    # Pitch=160 (toward aft of spacecraft) : [-0.940, 0, -0.342]
+    pitch_rad = np.deg2rad(pitch)
+    vec_sun_body = np.array([np.cos(pitch_rad), 0, -np.sin(pitch_rad)])
+    cxo_z_eci = np.dot(cxo_att.transform, vec_sun_body)
+
+    # Calculate the normal sun mode maneuver as the shortest manvr that points
+    # the Chandra -Z axis at the Sun. Note that for off-nominal roll this is NOT a pure
+    # pitch maneuver.
+    rot_angle = np.arccos(np.dot(cxo_z_eci, sun_eci))
+    rot_axis = np.cross(cxo_z_eci, sun_eci)
+    norm2 = np.dot(rot_axis, rot_axis)
+    if norm2 < 1e-16:
+        rot_axis = np.array([1.0, 0.0, 0.0])
+    else:
+        rot_axis /= np.sqrt(norm2)
+
+    sra = np.sin(rot_angle / 2) * rot_axis
+    manvr = Quat([sra[0], sra[1], sra[2], np.cos(rot_angle / 2)])
+
+    return manvr * cxo_att

ska_sun/tests/test_sun.py

@@ -10,6 +10,7 @@
 from ska_sun import (
     allowed_rolldev,
     apply_sun_pitch_yaw,
+    get_nsm_attitude,
     get_sun_pitch_yaw,
     nominal_roll,
     off_nominal_roll,
@@ -287,3 +288,53 @@ def test_array_input_and_different_formats(method):
         ra2, dec2 = ska_sun.position(time, method=method)
         assert np.isclose(ra, ra2, rtol=0, atol=1e-13)
         assert np.isclose(dec, dec2, rtol=0, atol=1e-13)
+
+
+@pytest.mark.parametrize("pitch", [90, 130, 160])
+def test_nsm_attitude_random_atts(pitch):
+    np.random.seed(0)
+    n_test = 10
+    date = "2024:001"
+    ras = np.random.uniform(0, 360, n_test)
+    decs = np.random.uniform(-90, 90, n_test)
+    rolls = [
+        ska_sun.nominal_roll(ra, dec, time=date) + np.random.uniform(-20, 20)
+        for ra, dec in zip(ras, decs)
+    ]
+
+    for ra, dec, roll in zip(ras, decs, rolls):
+        att0 = Quat([ra, dec, roll])
+        att_nsm = get_nsm_attitude(att0, date, pitch)
+        pitch_nsm = ska_sun.pitch(att_nsm.ra, att_nsm.dec, date)
+        assert np.isclose(pitch_nsm, pitch, rtol=0, atol=1e-4)
+        off_nom_roll_nsm = ska_sun.off_nominal_roll(att_nsm, date)
+        assert np.isclose(off_nom_roll_nsm, 0, rtol=0, atol=1e-4)
+
+
+def test_nsm_attitude_corner_case():
+    """Attitude which is already at exactly the NSM attitude.
+    This tests the lines::
+      if norm2 < 1e-16:
+          rot_axis = np.array([1., 0., 0.])
+    In development, a temporary print statement verified that this branch was taken.
+    Attitude is from::
+      ska_sun.get_att_for_sun_pitch_yaw(time="2024:001", pitch=90, yaw=85, off_nom_roll=0)
+    """
+    date = "2024:001"
+    att0 = Quat(
+        [
+            -0.5462715389868936,
+            -0.07466130207138927,
+            0.04050165010577328,
+            0.8332902927579349,
+        ]
+    )
+    att_nsm = get_nsm_attitude(att0, "2024:001")
+    pitch = ska_sun.pitch(att0.ra, att0.dec, date)
+    pitch_nsm = ska_sun.pitch(att_nsm.ra, att_nsm.dec, date)
+    assert np.isclose(pitch, 90, rtol=0, atol=1e-4)
+    assert np.isclose(pitch_nsm, 90, rtol=0, atol=1e-4)
+    off_nom_roll0 = ska_sun.off_nominal_roll(att0, date)
+    off_nom_roll_nsm = ska_sun.off_nominal_roll(att_nsm, date)
+    assert np.isclose(off_nom_roll0, 0, rtol=0, atol=1e-4)
+    assert np.isclose(off_nom_roll_nsm, 0, rtol=0, atol=1e-4)