Cahvor Frame Chain fix (#500)

* Fixed pointing in MSL driver

* Fixed cahvor model tests

ale/base/type_sensor.py

@@ -451,32 +451,7 @@ class Cahvor():
             v_s = self.compute_v_s()
             H_prime = (self.cahvor_camera_dict['H'] - h_c * self.cahvor_camera_dict['A'])/h_s
             V_prime = (self.cahvor_camera_dict['V'] - v_c * self.cahvor_camera_dict['A'])/v_s
-            r_matrix = np.array([H_prime, -V_prime, -self.cahvor_camera_dict['A']])
-
-            phi = math.asin(r_matrix[2][0])
-            w = - math.asin(r_matrix[2][1] / math.cos(phi))
-            k = math.acos(r_matrix[0][0] / math.cos(phi))
-
-            w = math.degrees(w)
-            phi = math.degrees(phi)
-            k = math.degrees(k)
-
-            # Rotational Matrix M generation
-            cahvor_rotation_matrix = np.zeros((3, 3))
-            cahvor_rotation_matrix[0, 0] = math.cos(phi) * math.cos(k)
-            cahvor_rotation_matrix[0, 1] = math.sin(w) * math.sin(phi) * math.cos(k) + \
-                math.cos(w) * math.sin(k)
-            cahvor_rotation_matrix[0, 2] = - math.cos(w) * math.sin(phi) * math.cos(k) + \
-                math.sin(w) * math.sin(k)
-            cahvor_rotation_matrix[1, 0] = - math.cos(phi) * math.sin(k)
-            cahvor_rotation_matrix[1, 1] = - math.sin(w) * math.sin(phi) * math.sin(k) + \
-                math.cos(w) * math.cos(k)
-            cahvor_rotation_matrix[1, 2] = math.cos(w) * math.sin(phi) * math.sin(k) + \
-                math.sin(w) * math.cos(k)
-            cahvor_rotation_matrix[2, 0] = math.sin(phi)
-            cahvor_rotation_matrix[2, 1] = - math.sin(w) * math.cos(phi)
-            cahvor_rotation_matrix[2, 2] = math.cos(w) * math.cos(phi)
-            self._cahvor_rotation_matrix = cahvor_rotation_matrix
+            self._cahvor_rotation_matrix = np.array([H_prime, -V_prime, -self.cahvor_camera_dict['A']])
         return self._cahvor_rotation_matrix
 
     @property
@@ -491,13 +466,13 @@ class Cahvor():
           A networkx frame chain object
         """
         if not hasattr(self, '_frame_chain'):
-            self._frame_chain = FrameChain.from_spice(sensor_frame=self.ikid,
+            self._frame_chain = FrameChain.from_spice(sensor_frame=self.spacecraft_id * 1000,
                                                       target_frame=self.target_frame_id,
                                                       center_ephemeris_time=self.center_ephemeris_time,
                                                       ephemeris_times=self.ephemeris_time,
                                                       nadir=False, exact_ck_times=False)
             cahvor_quats = Rotation.from_matrix(self.cahvor_rotation_matrix).as_quat()
-            cahvor_rotation = ConstantRotation(cahvor_quats, self.sensor_frame_id, self.ikid)
+            cahvor_rotation = ConstantRotation(cahvor_quats, self.spacecraft_id * 1000, self.sensor_frame_id)
             self._frame_chain.add_edge(rotation = cahvor_rotation)
         return self._frame_chain
 

tests/pytests/test_cahvor_mixin.py

@@ -22,7 +22,7 @@ class test_cahvor_sensor(unittest.TestCase):
     def setUp(self):
         self.driver = Cahvor()
         self.driver.focal_length = 100
-        self.driver.ikid = -76220
+        self.driver.spacecraft_id = -76
         self.driver.sensor_frame_id = -76562
         self.driver.target_frame_id = 10014
         self.driver.center_ephemeris_time = 0
@@ -38,19 +38,21 @@ class test_cahvor_sensor(unittest.TestCase):
     @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
     def test_cahvor_model_elements(self, cahvor_camera_dict):
         cahvor_matrix = self.driver.cahvor_rotation_matrix
-        np.testing.assert_allclose(cahvor_matrix, [[-0.42447558, -0.7572992,  -0.49630475],
-                                                   [ 0.73821222,  0.02793007, -0.67399009],
-                                                   [ 0.52427398, -0.65247056,  0.54719189]])
+        print(cahvor_matrix)
+        np.testing.assert_allclose(cahvor_matrix, [[-0.82067034, -0.57129702, 0.01095033],
+                                                   [-0.43920248, 0.6184257, -0.65165238],
+                                                   [ 0.3655151, -0.5396012, -0.7584387 ]])
 
     @patch('ale.transformation.FrameChain.from_spice', return_value=ale.transformation.FrameChain())
     @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
     def test_cahvor_frame_chain(self, cahvor_camera_dict, from_spice):
       frame_chain = self.driver.frame_chain
       assert len(frame_chain.nodes()) == 2
-      assert -76220 in frame_chain.nodes()
+      assert -76000 in frame_chain.nodes()
       assert -76562 in frame_chain.nodes()
-      from_spice.assert_called_with(center_ephemeris_time=0, ephemeris_times=[0], sensor_frame=-76220, target_frame=10014, nadir=False, exact_ck_times=False)
-      np.testing.assert_allclose(frame_chain[-76562][-76220]['rotation'].quat, [0.0100307131, -0.4757136116, 0.6970899144, 0.5363409323])
+      from_spice.assert_called_with(center_ephemeris_time=0, ephemeris_times=[0], sensor_frame=-76000, target_frame=10014, nadir=False, exact_ck_times=False)
+      print(frame_chain[-76562][-76000]['rotation'].quat)
+      np.testing.assert_allclose(frame_chain[-76562][-76000]['rotation'].quat, [-0.28255205, 0.8940826, -0.33309383, 0.09914206])
 
     @patch("ale.base.type_sensor.Cahvor.cahvor_camera_dict", new_callable=PropertyMock, return_value=cahvor_camera_dict())
     def test_cahvor_detector_center_line(self, cahvor_camera_dict):