libexail 0.1.0

//! Navigation data block structs (bits 0–30 of the nav bitmask).

use super::{block_enum, parse_blocks};
use crate::error::Result;
use binrw::BinRead;
use serde::Serialize;
use std::io::Cursor;

block_enum! {
    /// A navigation data block (bits 0–30 of the nav bitmask).
    pub enum NavigationBlock {
        0  => AttitudeHeading(AttitudeHeading, 12),
        1  => AttitudeHeadingStdDev(AttitudeHeadingStdDev, 12),
        2  => HeaveSurgeSway(HeaveSurgeSway, 16),
        3  => SmartHeave(SmartHeave, 8),
        4  => HeadingRollPitchRate(HeadingRollPitchRate, 12),
        5  => RotationRateVesselFrame(RotationRateVesselFrame, 12),
        6  => AccelerationVesselFrame(AccelerationVesselFrame, 12),
        7  => Position(Position, 21),
        8  => PositionStdDev(PositionStdDev, 16),
        9  => SpeedGeographic(SpeedGeographic, 12),
        10 => SpeedStdDev(SpeedStdDev, 12),
        11 => CurrentGeographic(CurrentGeographic, 8),
        12 => CurrentStdDev(CurrentStdDev, 8),
        13 => SystemDate(SystemDate, 4),
        14 => SensorStatus(SensorStatus, 8),
        15 if version >= 6 => InsAlgorithmStatusV6(InsAlgorithmStatusV6, 24),
        15 => InsAlgorithmStatusV5(InsAlgorithmStatusV5, 16),
        16 => InsSystemStatus(InsSystemStatus, 12),
        17 => InsUserStatus(InsUserStatus, 4),
        18 => AhrsAlgorithmStatus(AhrsAlgorithmStatus, 4),
        19 => AhrsSystemStatus(AhrsSystemStatus, 12),
        20 => AhrsUserStatus(AhrsUserStatus, 4),
        21 => HeaveSurgeSwaySpeed(HeaveSurgeSwaySpeed, 12),
        22 => SpeedVesselFrame(SpeedVesselFrame, 12),
        23 => AccelerationGeographic(AccelerationGeographic, 12),
        24 => CourseSpeedOverGround(CourseSpeedOverGround, 8),
        25 => Temperatures(Temperatures, 12),
        26 => AttitudeQuaternion(AttitudeQuaternion, 16),
        27 => AttitudeQuaternionStdDev(AttitudeQuaternionStdDev, 12),
        28 => RawAccelerationVesselFrame(RawAccelerationVesselFrame, 12),
        29 => AccelerationStdDev(AccelerationStdDev, 12),
        30 => RotationRateStdDev(RotationRateStdDev, 12),
    }
    read_fn(version: u8);
}

/// Parse navigation blocks from the cursor given the bitmask and protocol version.
pub fn parse_navigation_blocks(
    bitmask: u32,
    version: u8,
    cursor: &mut Cursor<&[u8]>,
) -> Result<Vec<NavigationBlock>> {
    parse_blocks(bitmask, 31, |bit| {
        NavigationBlock::read(bit, version, cursor).map(Some)
    })
}

/// Attitude & Heading (bit 0, 12 bytes).
/// Relates to vessel reference frame.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AttitudeHeading {
    /// In degrees, inside [0, 360).
    pub heading: f32,
    /// In degrees, positive when port up, inside [-180, 180).
    pub roll: f32,
    /// In degrees, positive bow down, inside [-90, 90].
    pub pitch: f32,
}

/// Attitude & Heading standard deviation (bit 1, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AttitudeHeadingStdDev {
    /// In degrees, inside [0, 360).
    pub heading_std_dev: f32,
    /// In degrees, inside [0, 360).
    pub roll_std_dev: f32,
    /// In degrees, inside [0, 360).
    pub pitch_std_dev: f32,
}

/// Real time Heave/Surge/Sway (bit 2, 16 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct HeaveSurgeSway {
    /// In meters, positive up (XV3H direction). Without lever arm, or COG heave
    /// if COG lever arm is specified.
    pub heave: f32,
    /// In meters, positive up (XV3H direction), at selected lever arm.
    pub heave_at_lever_arm: f32,
    /// In meters, positive forward in XV1H, at selected lever arm.
    pub surge: f32,
    /// In meters, positive port side in XV2H, at selected lever arm.
    pub sway: f32,
}

/// Smart Heave (bit 3, 8 bytes).
/// Delayed by 100s from real time.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct SmartHeave {
    /// Smart heave validity time in steps of 100 us.
    pub validity_time: u32,
    /// In meters, positive up (XV3H direction), at primary lever arm.
    pub heave: f32,
}

/// Heading/Roll/Pitch Rate (bit 4, 12 bytes).
/// Derivative of heading, roll, and pitch.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct HeadingRollPitchRate {
    /// In deg/s, positive when heading increases.
    pub heading_rate: f32,
    /// In deg/s, positive when port going up.
    pub roll_rate: f32,
    /// In deg/s, positive when bow going down.
    pub pitch_rate: f32,
}

/// Rotation rate in vessel frame (bit 5, 12 bytes).
/// Compensated from Earth rotation, gyro biases, and scale factors.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct RotationRateVesselFrame {
    /// In deg/s, positive when port is going up.
    pub xv1: f32,
    /// In deg/s, positive when bow is going down.
    pub xv2: f32,
    /// In deg/s, positive counter-clockwise around XV3.
    pub xv3: f32,
}

/// Acceleration in vessel frame, compensated from gravity (bit 6, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AccelerationVesselFrame {
    /// In m/s^2, positive forward.
    pub xv1: f32,
    /// In m/s^2, positive toward port.
    pub xv2: f32,
    /// In m/s^2, positive up.
    pub xv3: f32,
}

/// Position in WGS84 at selected lever arm (bit 7, 21 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct Position {
    /// In degrees, positive North, inside [-90, 90].
    pub latitude: f64,
    /// In degrees, increasing toward East, inside [0, 360).
    pub longitude: f64,
    /// 0: Geoid (Mean Sea Level), 1: Ellipsoid.
    pub altitude_reference: u8,
    /// In meters, positive up.
    pub altitude: f32,
}

/// Position standard deviation (bit 8, 16 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct PositionStdDev {
    /// In meters.
    pub north_std_dev: f32,
    /// In meters.
    pub east_std_dev: f32,
    /// Dimensionless.
    pub north_east_correlation: f32,
    /// In meters.
    pub altitude_std_dev: f32,
}

/// Speed in geographic frame (bit 9, 12 bytes).
/// At primary lever arm.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct SpeedGeographic {
    /// In m/s, positive north.
    pub north: f32,
    /// In m/s, positive east.
    pub east: f32,
    /// In m/s, positive up.
    pub up: f32,
}

/// Speed standard deviation in geographic frame (bit 10, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct SpeedStdDev {
    /// In m/s.
    pub north_std_dev: f32,
    /// In m/s.
    pub east_std_dev: f32,
    /// In m/s.
    pub up_std_dev: f32,
}

/// Current in geographic frame (bit 11, 8 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct CurrentGeographic {
    /// In m/s, positive north.
    pub north: f32,
    /// In m/s, positive east.
    pub east: f32,
}

/// Current standard deviation in geographic frame (bit 12, 8 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct CurrentStdDev {
    /// In m/s.
    pub north_std_dev: f32,
    /// In m/s.
    pub east_std_dev: f32,
}

/// System date (bit 13, 4 bytes).
/// Internal date if not GNSS-synchronized, otherwise ZDA date.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct SystemDate {
    /// Day inside [0, 31], 0 if unavailable.
    pub day: u8,
    /// Month inside [0, 12], 0 if unavailable.
    pub month: u8,
    /// Year inside [0, 65535], 0 if unavailable.
    pub year: u16,
}

/// INS/AHRS sensor status (bit 14, 8 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct SensorStatus {
    /// Sensor status word 1.
    pub status1: u32,
    /// Sensor status word 2.
    pub status2: u32,
}

/// INS algorithm status for protocol versions 2-5 (bit 15, 16 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct InsAlgorithmStatusV5 {
    /// Algorithm status word 1.
    pub status1: u32,
    /// Algorithm status word 2.
    pub status2: u32,
    /// Algorithm status word 3.
    pub status3: u32,
    /// Algorithm status word 4.
    pub status4: u32,
}

/// INS algorithm status for protocol version 6 (bit 15, 24 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct InsAlgorithmStatusV6 {
    /// Algorithm status word 1.
    pub status1: u32,
    /// Algorithm status word 2.
    pub status2: u32,
    /// Algorithm status word 3.
    pub status3: u32,
    /// Algorithm status word 4.
    pub status4: u32,
    /// Algorithm status word 5.
    pub status5: u32,
    /// Algorithm status word 6.
    pub status6: u32,
}

/// INS system status (bit 16, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct InsSystemStatus {
    /// System status word 1.
    pub status1: u32,
    /// System status word 2.
    pub status2: u32,
    /// System status word 3.
    pub status3: u32,
}

/// INS user status (bit 17, 4 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct InsUserStatus {
    /// User status word.
    pub status: u32,
}

/// AHRS algorithm status (bit 18, 4 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AhrsAlgorithmStatus {
    /// Algorithm status word.
    pub status: u32,
}

/// AHRS system status (bit 19, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AhrsSystemStatus {
    /// System status word 1.
    pub status1: u32,
    /// System status word 2.
    pub status2: u32,
    /// System status word 3.
    pub status3: u32,
}

/// AHRS user status (bit 20, 4 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AhrsUserStatus {
    /// User status word.
    pub status: u32,
}

/// Heave/Surge/Sway speed (bit 21, 12 bytes).
/// At selected lever arm.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct HeaveSurgeSwaySpeed {
    /// In m/s, positive up in XV3H direction.
    pub heave_speed: f32,
    /// In m/s, positive forward in XV1H.
    pub surge_speed: f32,
    /// In m/s, positive port side in XV2H.
    pub sway_speed: f32,
}

/// Speed in vessel frame (bit 22, 12 bytes).
/// At primary lever arm.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct SpeedVesselFrame {
    /// In m/s, positive along XV1.
    pub xv1: f32,
    /// In m/s, positive along XV2.
    pub xv2: f32,
    /// In m/s, positive along XV3.
    pub xv3: f32,
}

/// Acceleration in geographic frame, not compensated from gravity (bit 23, 12 bytes).
/// At primary lever arm.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AccelerationGeographic {
    /// In m/s^2, positive North.
    pub north: f32,
    /// In m/s^2, positive East.
    pub east: f32,
    /// In m/s^2, positive up.
    pub up: f32,
}

/// Course and speed over ground (bit 24, 8 bytes).
/// At primary lever arm.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct CourseSpeedOverGround {
    /// In degrees, inside [0, 360).
    pub course: f32,
    /// In m/s.
    pub speed: f32,
}

/// Temperatures (bit 25, 12 bytes).
/// Average sensor temperatures.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct Temperatures {
    /// Mean FOG temperature in degrees Celsius.
    pub fog: f32,
    /// Mean accelerometer temperature in degrees Celsius.
    pub acc: f32,
    /// Sensor board temperature in degrees Celsius.
    pub board: f32,
}

/// Attitude quaternion (bit 26, 16 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AttitudeQuaternion {
    /// Quaternion component q0 (scalar).
    pub q0: f32,
    /// Quaternion component q1.
    pub q1: f32,
    /// Quaternion component q2.
    pub q2: f32,
    /// Quaternion component q3.
    pub q3: f32,
}

/// Attitude quaternion standard deviation (bit 27, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AttitudeQuaternionStdDev {
    /// Standard deviation of error vector component e1.
    pub e1: f32,
    /// Standard deviation of error vector component e2.
    pub e2: f32,
    /// Standard deviation of error vector component e3.
    pub e3: f32,
}

/// Raw acceleration in vessel frame, not compensated from gravity (bit 28, 12 bytes).
/// At primary lever arm.
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct RawAccelerationVesselFrame {
    /// In m/s^2, positive forward.
    pub xv1: f32,
    /// In m/s^2, positive toward port.
    pub xv2: f32,
    /// In m/s^2, positive up.
    pub xv3: f32,
}

/// Acceleration standard deviation in vessel frame (bit 29, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct AccelerationStdDev {
    /// In m/s^2.
    pub xv1: f32,
    /// In m/s^2.
    pub xv2: f32,
    /// In m/s^2.
    pub xv3: f32,
}

/// Rotation rate standard deviation in vessel frame (bit 30, 12 bytes).
#[derive(Debug, Clone, BinRead, Serialize)]
#[brw(big)]
pub struct RotationRateStdDev {
    /// In deg/s.
    pub xv1: f32,
    /// In deg/s.
    pub xv2: f32,
    /// In deg/s.
    pub xv3: f32,
}