Extend reg.drone.physics.kinematics with 3D geometry and define the GNSS service

.travis.yml

@@ -4,4 +4,4 @@ python:
 before_install:
   - pip install pydsdl
 script:
-  - ./test.py
+  - ./verify.py

reg/drone/physics/kinematics/README.md

@@ -2,7 +2,35 @@
 
 This namespace contains data types that model basic [kinematic](https://en.wikipedia.org/wiki/Kinematics) states.
 
-A full kinematic state of a rigid body or fluid includes its position, velocity, acceleration, and orientation.
+The full kinematic state of a rigid body or fluid includes its position, velocity, acceleration, and orientation.
 The data types contained here model either full or partial kinematic states (e.g., there are types for velocity only).
 
 Forces acting on the body or fluid are part of its *dynamic* state, so they are excluded from the model.
+
+The modeled kinematic state of a body in `cartesian` space includes its pose (position and orientation)
+and twist (here, twist is understood narrowly as the translational and rotational velocity of the body).
+A "point state" lacks the rotational component (only position and velocity are defined).
+Translational components are always specified before their rotational counterparts.
+
+There is a dedicated namespace `geodetic` that defines position in spherical coordinates instead of Cartesian.
+This is to support large-scale navigation along the surface of celestial bodies where a local
+Cartesian approximation is infeasible.
+Other than using a different coordinate system to express position, the data types are equal to their Cartesian
+counterparts and are to some extent interchangeable thanks to the structural subtyping/aliasing features of DSDL.
+
+See UAVCAN Specification chapter "Application layer" for the applicable conventions.
+Key excerpts:
+
+- For world fixed frames, the North-East-Down (NED) right-handed notation is preferred:
+  X – northward, Y – eastward, Z – downward.
+
+- In relation to a body, the convention is as follows, right-handed:
+  X – forward, Y – rightward, Z – downward.
+
+- Angular velocities are represented using the right-handed, fixed-axis (extrinsic) convention:
+  X (roll), Y (pitch), Z (yaw).
+
+- For NED frames, the initial (zero) rotation of a body is the state where the axes of the body frame are
+  aligned with the axes of the local NED frame: X points north, Y points east, Z points down.
+
+- Matrices are represented in the row-major order.

reg/drone/physics/kinematics/cartesian/Point.0.1.uavcan

@@ -0,0 +1,5 @@
+# Cartesian coordinates of a point in space.
+
+uavcan.si.unit.length.WideVector3.1.0 value
+
+@sealed

reg/drone/physics/kinematics/cartesian/PointState.0.1.uavcan

@@ -0,0 +1,7 @@
+# The kinematic state of a point, as opposed to that of a body, is devoid of rotation information.
+# Therefore, the velocity is specified in the parent coordinate frame.
+
+Point.0.1 position
+uavcan.si.unit.velocity.Vector3.1.0 velocity
+
+@sealed

reg/drone/physics/kinematics/cartesian/PointStateVar.0.1.uavcan

@@ -0,0 +1,6 @@
+# See PointState for details.
+
+PointVar.0.1 position
+reg.drone.physics.kinematics.translation.Velocity3Var.0.1 velocity
+
+@sealed

reg/drone/physics/kinematics/cartesian/PointStateVarTs.0.1.uavcan

@@ -0,0 +1,5 @@
+uavcan.time.SynchronizedTimestamp.1.0 timestamp
+PointStateVar.0.1 value
+
+@sealed
+@assert _offset_.count == 1  # This is a fixed-length type.

reg/drone/physics/kinematics/cartesian/PointVar.0.1.uavcan

@@ -0,0 +1,4 @@
+Point.0.1 value
+float16[6] covariance_urt   # [meter^2]  Upper-right triangle of the covariance matrix.
+
+@sealed

reg/drone/physics/kinematics/cartesian/Pose.0.1.uavcan

@@ -0,0 +1,4 @@
+Point.0.1 position
+uavcan.si.unit.angle.Quaternion.1.0 orientation
+
+@sealed

reg/drone/physics/kinematics/cartesian/PoseVar.0.1.uavcan

@@ -0,0 +1,17 @@
+Pose.0.1 value
+
+float16[21] covariance_urt
+# Upper-right triangle of the covariance matrix:
+#
+#                    [parent frame]        [child (body) frame]
+#                translation along axis    rotation about axis
+#                   X       Y       Z       X       Y       Z
+#               +-----------------------------------------------
+# X position    |
+# Y position    |          m^2                     m*rad
+# Z position    |
+# X rotation    |
+# Y rotation    |                                  rad^2
+# Z rotation    |
+
+@sealed

reg/drone/physics/kinematics/cartesian/PoseVarTs.0.1.uavcan

@@ -0,0 +1,4 @@
+uavcan.time.SynchronizedTimestamp.1.0 timestamp
+PoseVar.0.1 value
+
+@sealed

reg/drone/physics/kinematics/cartesian/State.0.1.uavcan

@@ -0,0 +1,9 @@
+# First-order kinematic state of a body in space: pose and twist.
+# The pose defines a coordinate system transformation from the parent frame to the child frame.
+# The twist is specified in the child frame (body frame).
+
+Pose.0.1  pose
+Twist.0.1 twist
+
+@sealed
+@assert _offset_.count == 1  # This is a fixed-length type.

reg/drone/physics/kinematics/cartesian/StateVar.0.1.uavcan

@@ -0,0 +1,7 @@
+# See State for details. This type extends it with covariance matrices.
+
+PoseVar.0.1  pose
+TwistVar.0.1 twist
+
+@sealed
+@assert _offset_.count == 1  # This is a fixed-length type.

reg/drone/physics/kinematics/cartesian/StateVarTs.0.1.uavcan

@@ -0,0 +1,5 @@
+uavcan.time.SynchronizedTimestamp.1.0 timestamp
+StateVar.0.1 value
+
+@sealed
+@assert _offset_.count == 1  # This is a fixed-length type.

reg/drone/physics/kinematics/cartesian/Twist.0.1.uavcan

@@ -0,0 +1,9 @@
+# Motion of a rigid body in 3D space defined in the body frame.
+
+uavcan.si.unit.velocity.Vector3.1.0 linear
+# Linear velocity in the body frame.
+
+uavcan.si.unit.angular_velocity.Vector3.1.0 angular
+# Angular velocity about the fixed axes of the body frame (extrinsic).
+
+@sealed

reg/drone/physics/kinematics/cartesian/TwistVar.0.1.uavcan

@@ -0,0 +1,16 @@
+Twist.0.1 value
+
+float16[21] covariance_urt
+# Upper-right triangle of the covariance matrix:
+#
+#                     translation along axis    rotation about axis
+#                        X       Y       Z       X       Y       Z
+#                     +----------------------------------------------
+# X velocity          |
+# Y velocity          |      (m/s)^2                (m*rad)/s^2
+# Z velocity          |
+# X angular velocity  |
+# Y angular velocity  |                              (rad/s)^2
+# Z angular velocity  |
+
+@sealed

reg/drone/physics/kinematics/cartesian/TwistVarTs.0.1.uavcan

@@ -0,0 +1,4 @@
+uavcan.time.SynchronizedTimestamp.1.0 timestamp
+TwistVar.0.1 value
+
+@sealed

reg/drone/physics/kinematics/geodetic/Point.0.1.uavcan

@@ -0,0 +1,12 @@
+# Geodetic position: latitude, longitude, and altitude.
+# The order is chosen to match the axis ordering of the NED frame.
+# The size and layout of this structure is equal to the Cartesian pose type.
+
+float64 latitude   # [radian]
+float64 longitude  # [radian]
+
+uavcan.si.unit.length.WideScalar.1.0 altitude
+# Distance between the local mean sea level (MSL) and the focal point of the antenna. Positive altitude above the MSL.
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.Point.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/PointState.0.1.uavcan

@@ -0,0 +1,8 @@
+# The kinematic state of a point, as opposed to that of a body, is devoid of rotation information.
+# Therefore, the velocity is specified in the parent coordinate frame.
+
+Point.0.1 position
+uavcan.si.unit.velocity.Vector3.1.0 velocity
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.PointState.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/PointStateVar.0.1.uavcan

@@ -0,0 +1,7 @@
+# See PointState for details.
+
+PointVar.0.1 position
+reg.drone.physics.kinematics.translation.Velocity3Var.0.1 velocity
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.PointStateVar.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/PointStateVarTs.0.1.uavcan

@@ -0,0 +1,5 @@
+uavcan.time.SynchronizedTimestamp.1.0 timestamp
+PointStateVar.0.1 value
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.PointStateVarTs.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/PointVar.0.1.uavcan

@@ -0,0 +1,9 @@
+Point.0.1 value
+
+float16[6] covariance_urt   # [meter^2]
+# Upper-right triangle of the covariance matrix.
+# The position covariance is defined relative to a tangential plane through the specified latitude/longitude.
+# Element ordering: latitude, longitude, altitude. It is chosen to match the axis ordering of the NED frame.
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.PointVar.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/Pose.0.1.uavcan

@@ -0,0 +1,8 @@
+# Zero rotation is the state where the axes of the body frame are aligned with the axes of the local NED frame:
+# X points north, Y points east, Z points down.
+
+Point.0.1                           position
+uavcan.si.unit.angle.Quaternion.1.0 orientation
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.Pose.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/PoseVar.0.1.uavcan

@@ -0,0 +1,17 @@
+Pose.0.1 value
+float16[21] covariance_urt
+# Upper-right triangle of the covariance matrix:
+#
+#                    [parent frame]        [child (body) frame]
+#               translation along axis     rotation about axis
+#                   X       Y       Z       X       Y       Z
+#               +-----------------------------------------------
+# X position    |
+# Y position    |          m^2                     m*rad
+# Z position    |
+# X rotation    |
+# Y rotation    |                                  rad^2
+# Z rotation    |
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.PoseVar.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/State.0.1.uavcan

@@ -0,0 +1,9 @@
+# First-order kinematic state of a body near the surface of a planet.
+# The pose defines a coordinate system transformation from the parent frame to the child frame.
+# The twist is specified in the child frame (body frame).
+
+Pose.0.1 pose
+reg.drone.physics.kinematics.cartesian.Twist.0.1 twist
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.State.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/StateVar.0.1.uavcan

@@ -0,0 +1,7 @@
+# See State for details. This type extends it with covariance matrices.
+
+PoseVar.0.1 pose
+reg.drone.physics.kinematics.cartesian.TwistVar.0.1 twist
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.StateVar.0.1._bit_length_

reg/drone/physics/kinematics/geodetic/StateVarTs.0.1.uavcan

@@ -0,0 +1,5 @@
+uavcan.time.SynchronizedTimestamp.1.0 timestamp
+StateVar.0.1 value
+
+@sealed
+@assert _offset_ == reg.drone.physics.kinematics.cartesian.StateVarTs.0.1._bit_length_

reg/drone/physics/kinematics/rotation/Planar.0.1.uavcan

@@ -1,5 +1,5 @@
 # Rotation about an axis.
-uavcan.si.unit.angle.Scalar.1.0                angular_position         # Required
-uavcan.si.unit.angular_velocity.Scalar.1.0     angular_velocity         # Required, best guess if unknown
-uavcan.si.unit.angular_acceleration.Scalar.1.0 angular_acceleration     # Required, best guess if unknown
+uavcan.si.unit.angle.Scalar.1.0                angular_position
+uavcan.si.unit.angular_velocity.Scalar.1.0     angular_velocity
+uavcan.si.unit.angular_acceleration.Scalar.1.0 angular_acceleration
 @sealed

reg/drone/physics/kinematics/translation/Linear.0.1.uavcan

@@ -1,5 +1,5 @@
 # Movement along an axis.
-uavcan.si.unit.length.Scalar.1.0       position         # Required
-uavcan.si.unit.velocity.Scalar.1.0     velocity         # Required, best guess if unknown
-uavcan.si.unit.acceleration.Scalar.1.0 acceleration     # Required, best guess if unknown
+uavcan.si.unit.length.Scalar.1.0       position
+uavcan.si.unit.velocity.Scalar.1.0     velocity
+uavcan.si.unit.acceleration.Scalar.1.0 acceleration
 @sealed

reg/drone/physics/kinematics/translation/Velocity1VarTs.0.1.uavcan

@@ -2,7 +2,7 @@
 # Observe that this is a structural subtype of uavcan.si.sample.velocity.Scalar.1.0.
 # For a non-timestamped estimate without covariance use the raw SI type directly.
 
-uavcan.si.sample.velocity.Scalar.1.0 velocity
+uavcan.si.sample.velocity.Scalar.1.0 value
 
 float16 error_variance  # [(meter/second)^2]
 

reg/drone/physics/kinematics/translation/Velocity3Var.0.1.uavcan

@@ -0,0 +1,7 @@
+# Linear velocity with covariance.
+# Observe that this is a structural subtype of uavcan.si.unit.velocity.Scalar.1.0.
+
+uavcan.si.sample.velocity.Vector3.1.0 value
+float16[6] covariance_urt  # [(meter/second)^2] Upper-right triangle of the covariance matrix.
+
+@sealed

reg/drone/physics/thermodynamics/PressureTempVarTs.0.1.uavcan

@@ -6,7 +6,7 @@ uavcan.time.SynchronizedTimestamp.1.0 timestamp
 uavcan.si.unit.pressure.Scalar.1.0    pressure
 uavcan.si.unit.temperature.Scalar.1.0 temperature
 
-float16[3] covariance
+float16[3] covariance_urt
 # The upper-right triangle of the covariance matrix (following the matrix packing rules defined in Specification).
 #   0 -- pascal^2
 #   1 -- pascal*kelvin

reg/drone/physics/time/TAI64.0.1.uavcan

@@ -0,0 +1,20 @@
+# Standard TAI64N time label (https://cr.yp.to/libtai/tai64.html). Quote from the source:
+#
+#   TAI stands for Temps Atomique International, the current international real-time standard.
+#   One TAI second is defined as the duration of 9192631770 periods of the radiation corresponding
+#   to the transition between the two hyperfine levels of the ground state of the cesium atom.
+#   TAI also specifies a frame of reference. Further discussion of special relativity is outside
+#   the scope of this document.
+#
+#   A TAI64 label is an integer between 0 and 2^64 referring to a particular second of real time. Integer s refers to:
+#
+#       - the TAI second beginning exactly 2^62 - s seconds before the beginning of 1970 TAI,
+#         if s is between 0 inclusive and 2^62 exclusive; or
+#
+#       - the TAI second beginning exactly s - 2^62 seconds after the beginning of 1970 TAI,
+#         if s is between 2^62 inclusive and 2^63 exclusive.
+#
+
+int64 tai64n    # [nanosecond] Nanoseconds elapsed since 1970-01-01T00:00:00Z TAI.
+
+@sealed

reg/drone/physics/time/TAI64Var.0.1.uavcan

@@ -0,0 +1,8 @@
+TAI64.0.1 value
+
+float32 error_variance      # [second^2]
+# Error variance, in second squared, of the time estimate.
+# Infinity indicates that the time estimates are not yet available.
+# A non-positive value indicates that the error variance is unknown.
+
+@sealed

reg/drone/physics/time/TAI64VarTs.0.1.uavcan

@@ -0,0 +1,4 @@
+uavcan.time.SynchronizedTimestamp.1.0 timestamp
+TAI64Var.0.1 value
+
+@sealed

reg/drone/service/gnss/DilutionOfPrecision.0.1.uavcan

@@ -0,0 +1,19 @@
+# The standard DOP estimates (https://en.wikipedia.org/wiki/Dilution_of_precision).
+# DOP values should not be confused with accuracy estimates -- those are expressed through the covariance matrices.
+#
+# Applicable relations:
+#   gdop = (ndop^2 + edop^2 + vdop^2 + tdop^2)^0.5
+#   pdop = (ndop^2 + edop^2 + vdop^2)^0.5
+#   hdop = (ndop^2 + edop^2)^0.5
+#
+# Fields whose values are unknown should be set to NaN.
+
+float16 geometric   # GDOP
+float16 position    # PDOP
+float16 horizontal  # HDOP
+float16 vertical    # VDOP
+float16 time        # TDOP
+float16 northing    # NDOP
+float16 easting     # EDOP
+
+@sealed

reg/drone/service/gnss/Heartbeat.0.1.uavcan

@@ -0,0 +1,23 @@
+# Optional, extended information on the performance of the GNSS sensor.
+
+reg.drone.service.common.Heartbeat.0.1 heartbeat
+
+Sources.0.1 sources
+
+DilutionOfPrecision.0.1 dop
+
+uint8 num_visible_satellites  # The number of space vehicles visible when the solution was computed.
+uint8 num_used_satellites     # The number of space vehicles whose signals have been utilized to derive the solution.
+
+bool fix
+# True if a GNSS fix is established (of any dimensionality, e.g., time-only).
+# Data consumers should not use this flag and rely on the error estimates reported via covariance matrices instead.
+# If the sensor is equipped with other means of deriving the navigation solution (dead reckoning, VIO, UWB, etc.),
+# then this flag may not be representative of the actual status of the solution (only matrices are).
+
+bool rtk_fix
+# True if the current mode is RTK and an RTK fix has been established.
+# This flag should not be set unless `fix` is also set.
+# The availability of base station data is indicated using a separate (orthogonal) flag defined in Sources.
+
+@extent 8 * 124