Merge branch 'main' into deconflict

Author: jbrodovsky

core/src/lib.rs

@@ -293,8 +293,8 @@ impl StrapdownState {
     /// * `altitude` - Altitude in meters.
     /// * `velocity_north` - North velocity in m/s.
     /// * `velocity_east` - East velocity in m/s.
-    /// * `velocity_vertical` - Vertical velocity in m/s (positive up in ENU, positive down in NED).
-    /// * `attitude` - `Rotation3<f64>` attitude matrix.
+    /// * `velocity_down` - Down velocity in m/s.
+    /// * `attitude` - Rotation3<f64> attitude matrix.
     /// * `in_degrees` - If true, angles are provided in degrees and will be converted to radians.
     #[allow(clippy::too_many_arguments)]
     pub fn new(
@@ -346,7 +346,7 @@ impl StrapdownState {
     // --- From/Into trait implementations for StrapdownState <-> Vec<f64> and &[f64] ---
 }
 impl From<StrapdownState> for Vec<f64> {
-    /// Converts a StrapdownState to a `Vec<f64>` in order assuming angles are in radians.
+    /// Converts a StrapdownState to a Vec<f64> in NED order, angles in radians.
     fn from(state: StrapdownState) -> Self {
         let (roll, pitch, yaw) = state.attitude.euler_angles();
         vec![
@@ -363,7 +363,7 @@ impl From<StrapdownState> for Vec<f64> {
     }
 }
 impl From<&StrapdownState> for Vec<f64> {
-    /// Converts a reference to StrapdownState to a `Vec<f64>` in NED order assuming angles are in radians.
+    /// Converts a reference to StrapdownState to a Vec<f64> in NED order, angles in radians.
     fn from(state: &StrapdownState) -> Self {
         let (roll, pitch, yaw) = state.attitude.euler_angles();
         vec![
@@ -396,19 +396,19 @@ impl TryFrom<&[f64]> for StrapdownState {
 }
 impl TryFrom<Vec<f64>> for StrapdownState {
     type Error = &'static str;
-    /// Attempts to create a StrapdownState from a `Vec<f64>` of length 9 assuming angles in radians.
+    /// Attempts to create a StrapdownState from a Vec<f64> of length 9 (NED order, radians).
     fn try_from(vec: Vec<f64>) -> Result<Self, Self::Error> {
         Self::try_from(vec.as_slice())
     }
 }
 impl From<StrapdownState> for DVector<f64> {
-    /// Converts a StrapdownState to a `DVector<f64>` in NED order assuming angles are in radians.
+    /// Converts a StrapdownState to a DVector<f64> in NED order, angles in radians.
     fn from(state: StrapdownState) -> Self {
         DVector::from_vec(state.into())
     }
 }
 impl From<&StrapdownState> for DVector<f64> {
-    /// Converts a reference to StrapdownState to a `DVector<f64>` in NED order assuming angles are in radians.
+    /// Converts a reference to StrapdownState to a DVector<f64> in NED order, angles in radians.
     fn from(state: &StrapdownState) -> Self {
         DVector::from_vec(state.into())
     }

core/src/messages.rs

@@ -1561,65 +1561,4 @@ mod serialization_tests {
         let loaded = GnssDegradationConfig::from_file(&path).unwrap();
         assert_eq!(cfg.seed, loaded.seed);
     }
-
-    #[test]
-    fn default_config_roundtrip() {
-        // Test that default config can be serialized and read back
-        let cfg = GnssDegradationConfig::default();
-
-        // Verify default values
-        assert_eq!(cfg.seed, 42);
-        assert!(matches!(cfg.scheduler, GnssScheduler::PassThrough));
-        assert!(matches!(cfg.fault, GnssFaultModel::None));
-
-        // Test JSON roundtrip
-        let f = NamedTempFile::new().unwrap();
-        let path = f.path().with_extension("json");
-        cfg.to_file(&path).unwrap();
-        let loaded = GnssDegradationConfig::from_file(&path).unwrap();
-        assert_eq!(cfg.seed, loaded.seed);
-        assert!(matches!(loaded.scheduler, GnssScheduler::PassThrough));
-        assert!(matches!(loaded.fault, GnssFaultModel::None));
-
-        // Test YAML roundtrip
-        let f2 = NamedTempFile::new().unwrap();
-        let path2 = f2.path().with_extension("yaml");
-        cfg.to_file(&path2).unwrap();
-        let loaded2 = GnssDegradationConfig::from_file(&path2).unwrap();
-        assert_eq!(cfg.seed, loaded2.seed);
-
-        // Test TOML roundtrip
-        let f3 = NamedTempFile::new().unwrap();
-        let path3 = f3.path().with_extension("toml");
-        cfg.to_file(&path3).unwrap();
-        let loaded3 = GnssDegradationConfig::from_file(&path3).unwrap();
-        assert_eq!(cfg.seed, loaded3.seed);
-    }
-
-    #[test]
-    fn unsupported_extension_error() {
-        let cfg = sample_cfg();
-        let f = NamedTempFile::new().unwrap();
-        let path = f.path().with_extension("txt");
-
-        // Test to_file with unsupported extension
-        let result = cfg.to_file(&path);
-        assert!(result.is_err());
-        assert_eq!(result.unwrap_err().kind(), std::io::ErrorKind::InvalidInput);
-
-        // Test from_file with unsupported extension
-        let result = GnssDegradationConfig::from_file(&path);
-        assert!(result.is_err());
-        assert_eq!(result.unwrap_err().kind(), std::io::ErrorKind::InvalidInput);
-    }
-
-    #[test]
-    fn yml_extension_roundtrip() {
-        let cfg = sample_cfg();
-        let f = NamedTempFile::new().unwrap();
-        let path = f.path().with_extension("yml");
-        cfg.to_file(&path).unwrap();
-        let loaded = GnssDegradationConfig::from_file(&path).unwrap();
-        assert_eq!(cfg.seed, loaded.seed);
-    }
 }

core/src/sim.rs

@@ -802,8 +802,11 @@ pub fn dead_reckoning(records: &[TestDataRecord]) -> Vec<NavigationResult> {
         is_enu: true,
     };
     // Store the initial state and metadata
-    info!("Initializing dead reckoning simulation at {}", state);
-    results.push(NavigationResult::from((&first_record.time, &state)));
+    results.push(NavigationResult::from((
+        &first_record.time,
+        &state.into(),
+        &DMatrix::from_diagonal(&DVector::from_element(15, 0.0)),
+    )));
     let mut previous_time = records[0].time;
     // Process each subsequent record
     for record in records.iter().skip(1) {
@@ -816,8 +819,11 @@ pub fn dead_reckoning(records: &[TestDataRecord]) -> Vec<NavigationResult> {
             gyro: Vector3::new(record.gyro_x, record.gyro_y, record.gyro_z),
         };
         forward(&mut state, imu_data, dt);
-        results.push(NavigationResult::from((&current_time, &state)));
-        info!("Updated state at {}: {:?}", current_time, state);
+        results.push(NavigationResult::from((
+            &current_time,
+            &state.into(),
+            &DMatrix::from_diagonal(&DVector::from_element(15, 0.0)),
+        )));
         previous_time = record.time;
     }
     results