measurements.h
Go to the documentation of this file.
1 /**************************************************************************/ /**
2  * @brief Sensor measurement messages.
3  * @file
4  ******************************************************************************/
5 
6 #pragma once
7 
10 
11 namespace point_one {
12 namespace fusion_engine {
13 namespace messages {
14 
15 // Enforce 4-byte alignment and packing of all data structures and values.
16 // Floating point values are aligned on platforms that require it. This is done
17 // with a combination of setting struct attributes, and manual alignment
18 // within the definitions. See the "Message Packing" section of the README.
19 #pragma pack(push, 1)
20 
21 /**
22  * @defgroup measurement_messages Sensor Measurement Message Definitions
23  * @brief Measurement data from available sensors.
24  * @ingroup messages
25  *
26  * See also @ref messages.
27  */
28 
29 /**
30  * @brief The source of received sensor measurements, if known.
31  * @ingroup measurement_messages
32  */
33 enum class SensorDataSource : uint8_t {
34  /** Data source not known. */
35  UNKNOWN = 0,
36  /**
37  * Sensor data captured internal to the device (embedded IMU, GNSS receiver,
38  * etc.).
39  */
40  INTERNAL = 1,
41  /**
42  * Sensor data generated via hardware voltage signal (wheel tick, external
43  * event, etc.).
44  */
45  HARDWARE_IO = 2,
46  /** Sensor data captured from a vehicle CAN bus. */
47  CAN = 3,
48  /** Sensor data provided over a serial connection. */
49  SERIAL = 4,
50  /** Sensor data provided over a network connection. */
51  NETWORK = 5,
52 };
53 
54 /**
55  * @brief Get a human-friendly string name for the specified @ref
56  * SensorDataSource.
57  * @ingroup measurement_messages
58  *
59  * @param val The enum to get the string name for.
60  *
61  * @return The corresponding string name.
62  */
64  switch (val) {
66  return "Unknown";
68  return "Internal";
70  return "Hardware I/O";
72  return "CAN";
74  return "Serial";
76  return "Network";
77  default:
78  return "Unrecognized";
79  }
80 }
81 
82 /**
83  * @brief @ref SensorDataSource stream operator.
84  * @ingroup measurement_messages
85  */
87  stream << to_string(val) << " (" << (int)val << ")";
88  return stream;
89 }
90 
91 /**
92  * @brief The source of a @ref point_one::fusion_engine::messages::Timestamp
93  * used to represent the time of applicability of an incoming sensor
94  * measurement.
95  * @ingroup measurement_messages
96  */
97 enum class SystemTimeSource : uint8_t {
98  /** Timestamp not valid. */
99  INVALID = 0,
100  /** Message timestamped in P1 time. */
101  P1_TIME = 1,
102  /**
103  * Message timestamped in system time, generated when received by the device.
104  */
106  /**
107  * Message timestamp was generated from a monotonic clock of an external
108  * system.
109  */
110  SENDER_SYSTEM_TIME = 3,
111  /**
112  * Message timestamped in GPS time, referenced to 1980/1/6.
113  */
114  GPS_TIME = 4,
115 };
116 
117 /**
118  * @brief Get a human-friendly string name for the specified @ref
119  * SystemTimeSource.
120  * @ingroup measurement_messages
121  *
122  * @param val The enum to get the string name for.
123  *
124  * @return The corresponding string name.
125  */
127  switch (val) {
129  return "Invalid";
131  return "P1 Time";
133  return "Timestamped on Reception";
135  return "Sender System Time";
137  return "GPS Time";
138  default:
139  return "Unrecognized";
140  }
141 }
142 
143 /**
144  * @brief @ref SystemTimeSource stream operator.
145  * @ingroup measurement_messages
146  */
148  stream << to_string(val) << " (" << (int)val << ")";
149  return stream;
150 }
151 
152 /**
153  * @brief The time of applicability and additional information for an incoming
154  * sensor measurement.
155  * @ingroup measurement_messages
156  *
157  * By convention this will be the first member of any message containing
158  * input measurements by the host to the device, as well as raw measurement
159  * outputs from the device.
160  *
161  * The @ref measurement_time field stores time of applicability/reception for
162  * the measurement data, expressed in one of the available source time bases
163  * (see @ref SystemTimeSource). The timestamp will be converted to P1 time
164  * automatically by FusionEngine using an internal model of P1 vs. the specified
165  * source time.
166  *
167  * @section meas_details_on_arrival Timestamp On Arrival
168  *
169  * On most platforms, incoming sensor measurements are timestamped automatically
170  * by FusionEngine when they arrive. To request timestamp on arrival, set @ref
171  * measurement_time_source to either @ref
172  * SystemTimeSource::TIMESTAMPED_ON_RECEPTION or @ref SystemTimeSource::INVALID.
173  *
174  * @section meas_details_external_time Timestamp Externally
175  *
176  * On some platforms, incoming sensor measurements may be timestamped
177  * externally by the host prior to arrival, either in GPS time (@ref
178  * SystemTimeSource::GPS_TIME), or using a monotonic clock controlled by the
179  * host system (@ref SystemTimeSource::SENDER_SYSTEM_TIME). For those platforms,
180  * the @ref measurement_time field should be specified in the incoming message.
181  *
182  * @note
183  * Use of an external monotonic clock requires additional coordination with the
184  * target FusionEngine device.
185  *
186  * @section meas_details_p1_time Timestamp With External P1 Time
187  *
188  * Measurements may only be timestamped externally using P1 time (@ref
189  * SystemTimeSource::P1_TIME) if the external system supports remote
190  * synchronization of the P1 time clock model. This is intended for internal
191  * use only.
192  */
194  /**
195  * The measurement time of applicability, if available, in a user-specified
196  * time base. The source of this value is specified in @ref
197  * measurement_time_source. The timestamp will be converted to P1 time
198  * internally by the device before use.
199  */
201 
202  /**
203  * The source for @ref measurement_time.
204  */
205  SystemTimeSource measurement_time_source = SystemTimeSource::INVALID;
206 
207  /**
208  * The source of the incoming data, if known.
209  */
211 
212  uint8_t reserved[2] = {0};
213 
214  /**
215  * The P1 time corresponding with the measurement time of applicability, if
216  * available.
217  *
218  * For inputs to the device, this field will be populated automatically by the
219  * device on arrival based on @ref measurement_time. Any existing value will
220  * be overwritten. To specify a known P1 time, specify the value in @ref
221  * measurement_time and set @ref measurement_time_source to @ref
222  * SystemTimeSource::P1_TIME.
223  *
224  * For outputs from the device, this field will always be populated with the
225  * P1 time corresponding with the measurement.
226  */
228 };
229 
230 ////////////////////////////////////////////////////////////////////////////////
231 // IMU Measurements
232 ////////////////////////////////////////////////////////////////////////////////
233 
234 /**
235  * @brief IMU sensor measurement input (@ref MessageType::IMU_INPUT,
236  * version 1.0).
237  * @ingroup measurement_messages
238  *
239  * This message is an input to the device containing raw IMU acceleration and
240  * rotation rate measurements.
241  *
242  * See also @ref IMUOutput.
243  */
244 struct P1_ALIGNAS(4) IMUInput : public MessagePayload {
245  static constexpr MessageType MESSAGE_TYPE = MessageType::IMU_INPUT;
246  static constexpr uint8_t MESSAGE_VERSION = 0;
247 
248  /**
249  * Measurement timestamp and additional information, if available. See @ref
250  * MeasurementDetails for details.
251  */
253 
254  uint8_t reserved[6] = {0};
255 
256  /**
257  * The IMU temperature (in deg Celcius * 2^-7). Set to 0x7FFF if invalid.
258  */
259  int16_t temperature = INT16_MAX;
260 
261  /**
262  * Measured x/y/z acceleration (in meters/second^2 * 2^-16), resolved in the
263  * sensor measurement frame. Set to 0x7FFFFFFF if invalid.
264  */
265  int32_t accel[3] = {INT32_MAX, INT32_MAX, INT32_MAX};
266 
267  /**
268  * Measured x/y/z rate of rotation (in radians/second * 2^-20), resolved in
269  * the sensor measurement frame. Set to 0x7FFFFFFF if invalid.
270  */
271  int32_t gyro[3] = {INT32_MAX, INT32_MAX, INT32_MAX};
272 };
273 
274 /**
275  * @brief IMU sensor measurement output with calibration and corrections applied
276  * (@ref MessageType::IMU_OUTPUT, version 1.0).
277  * @ingroup measurement_messages
278  *
279  * This message is an output from the device containing IMU acceleration and
280  * rotation rate measurements. The measurements been corrected for biases and
281  * scale factors, and have been rotated into the vehicle body frame from the
282  * original IMU orientation, including calibrated mounting error estimates.
283  *
284  * See also @ref RawIMUOutput.
285  */
286 struct P1_ALIGNAS(4) IMUOutput : public MessagePayload {
287  static constexpr MessageType MESSAGE_TYPE = MessageType::IMU_OUTPUT;
288  static constexpr uint8_t MESSAGE_VERSION = 0;
289 
290  /** The time of the measurement, in P1 time (beginning at power-on). */
292 
293  /**
294  * Corrected vehicle x/y/z acceleration (in meters/second^2), resolved in the
295  * body frame.
296  */
297  double accel_mps2[3] = {NAN, NAN, NAN};
298 
299  /**
300  * Corrected vehicle x/y/z acceleration standard deviation (in
301  * meters/second^2), resolved in the body frame.
302  */
303  double accel_std_mps2[3] = {NAN, NAN, NAN};
304 
305  /**
306  * Corrected vehicle x/y/z rate of rotation (in radians/second), resolved in
307  * the body frame.
308  */
309  double gyro_rps[3] = {NAN, NAN, NAN};
310 
311  /**
312  * Corrected vehicle x/y/z rate of rotation standard deviation (in
313  * radians/second), resolved in the body frame.
314  */
315  double gyro_std_rps[3] = {NAN, NAN, NAN};
316 };
317 
318 /**
319  * @brief Raw (uncorrected) IMU sensor measurement output (@ref
320  MessageType::RAW_IMU_OUTPUT, version 1.0).
321  * @ingroup measurement_messages
322  *
323  * This message is an output from the device containing raw IMU acceleration and
324  * rotation rate measurements. These measurements come directly from the sensor,
325  * and do not have any corrections or calibration applied.
326  *
327  * See also @ref IMUOutput.
328  */
330  static constexpr MessageType MESSAGE_TYPE = MessageType::RAW_IMU_OUTPUT;
331  static constexpr uint8_t MESSAGE_VERSION = 0;
332 
333  /**
334  * Measurement timestamp and additional information, if available. See @ref
335  * MeasurementDetails for details.
336  */
338 
339  uint8_t reserved[6] = {0};
340 
341  /**
342  * The IMU temperature (in deg Celcius * 2^-7). Set to 0x7FFF if invalid.
343  */
344  int16_t temperature = INT16_MAX;
345 
346  /**
347  * Measured x/y/z acceleration (in meters/second^2 * 2^-16), resolved in the
348  * sensor measurement frame. Set to 0x7FFFFFFF if invalid.
349  */
350  int32_t accel[3] = {INT32_MAX, INT32_MAX, INT32_MAX};
351 
352  /**
353  * Measured x/y/z rate of rotation (in radians/second * 2^-20), resolved in
354  * the sensor measurement frame. Set to 0x7FFFFFFF if invalid.
355  */
356  int32_t gyro[3] = {INT32_MAX, INT32_MAX, INT32_MAX};
357 };
358 
359 ////////////////////////////////////////////////////////////////////////////////
360 // Different Wheel Speed Measurements
361 ////////////////////////////////////////////////////////////////////////////////
362 
363 /**
364  * @brief The current transmission gear used by the vehicle.
365  * @ingroup measurement_messages
366  */
367 enum class GearType : uint8_t {
368  /**
369  * The transmission gear is not known, or does not map to a supported
370  * GearType.
371  */
372  UNKNOWN = 0,
373  FORWARD = 1, ///< The vehicle is in a forward gear.
374  REVERSE = 2, ///< The vehicle is in reverse.
375  PARK = 3, ///< The vehicle is parked.
376  NEUTRAL = 4, ///< The vehicle is in neutral.
377 };
378 
379 /**
380  * @brief Get a human-friendly string name for the specified @ref GearType.
381  * @ingroup measurement_messages
382  *
383  * @param val The enum to get the string name for.
384  *
385  * @return The corresponding string name.
386  */
388  switch (val) {
389  case GearType::UNKNOWN:
390  return "Unknown";
391  case GearType::FORWARD:
392  return "Forward";
393  case GearType::REVERSE:
394  return "Reverse";
395  case GearType::PARK:
396  return "Park";
397  case GearType::NEUTRAL:
398  return "Neutral";
399  default:
400  return "Unrecognized";
401  }
402 }
403 
404 /**
405  * @brief @ref GearType stream operator.
406  * @ingroup measurement_messages
407  */
408 inline p1_ostream& operator<<(p1_ostream& stream, GearType val) {
409  stream << to_string(val) << " (" << (int)val << ")";
410  return stream;
411 }
412 
413 /**
414  * @brief Differential wheel speed measurement input (@ref
415  * MessageType::WHEEL_SPEED_INPUT, version 1.0).
416  * @ingroup measurement_messages
417  *
418  * This message is an input to the device, used to convey the speed of each
419  * individual wheel on the vehicle. The number and type of wheels expected
420  * varies by vehicle. For single along-track speed measurements, see @ref
421  * VehicleSpeedInput.
422  *
423  * To use wheel speed data, you must first configure the device by issuing a
424  * @ref SetConfigMessage message containing a @ref WheelConfig payload
425  * describing the vehicle sensor configuration (speed data signed/unsigned,
426  * etc.).
427  *
428  * Some platforms may have an additional voltage signal used to indicate
429  * direction of motion, have direction or gear information available from a
430  * vehicle CAN bus, etc. If direction/gear information is available, it may be
431  * provided in the @ref gear field.
432  *
433  * To send wheel tick counts from software, use @ref WheelTickInput instead.
434  *
435  * See also @ref WheelSpeedOutput for measurement output.
436  */
438  static constexpr MessageType MESSAGE_TYPE = MessageType::WHEEL_SPEED_INPUT;
439  static constexpr uint8_t MESSAGE_VERSION = 0;
440 
441  /**
442  * Set this flag if the measured wheel speeds are signed (positive forward,
443  * negative reverse). Otherwise, if the values are assumed to be unsigned
444  * (positive in both directions).
445  */
446  static constexpr uint8_t FLAG_SIGNED = 0x1;
447 
448  /**
449  * Measurement timestamp and additional information, if available. See @ref
450  * MeasurementDetails for details.
451  */
453 
454  /**
455  * The front left wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
456  * available.
457  */
458  int32_t front_left_speed = INT32_MAX;
459 
460  /**
461  * The front right wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
462  * available.
463  */
464  int32_t front_right_speed = INT32_MAX;
465 
466  /**
467  * The rear left wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
468  * available.
469  */
470  int32_t rear_left_speed = INT32_MAX;
471 
472  /**
473  * The rear right wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
474  * available.
475  */
476  int32_t rear_right_speed = INT32_MAX;
477 
478  /**
479  * The transmission gear currently in use, or direction of motion, if
480  * available.
481  *
482  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
483  * direction information is available externally.
484  */
486 
487  /** A bitmask of flags associated with the measurement data. */
488  uint8_t flags = 0x0;
489 
490  uint8_t reserved[2] = {0};
491 };
492 
493 /**
494  * @brief Differential wheel speed measurement output with calibration and
495  * corrections applied (@ref MessageType::WHEEL_SPEED_OUTPUT, version
496  1.0).
497  * @ingroup measurement_messages
498  *
499  * This message is an output from the device that contains the speed of each
500  * individual wheel on the vehicle, after applying any estimated corrections for
501  * wheel scale factor, sign, etc.
502  *
503  * Wheel odometry data may be received via a software input from a host machine,
504  * a vehicle CAN bus, or a hardware voltage signal (wheel ticks). The @ref
505  * data_source field will indicate which type of data source provided the
506  * measurements to the device.
507  *
508  * @note
509  * When odometry is provided using hardware wheel ticks, the output rate of this
510  * message may differ from the wheel tick input rate. For high accuracy
511  * applications, FusionEngine may integrate tick counts over longer intervals to
512  * improve performance.
513  *
514  * See also @ref WheelSpeedInput and @ref RawWheelSpeedOutput.
515  */
517  static constexpr MessageType MESSAGE_TYPE = MessageType::WHEEL_SPEED_OUTPUT;
518  static constexpr uint8_t MESSAGE_VERSION = 0;
519 
520  /**
521  * Set this flag if the measured wheel speeds are signed (positive forward,
522  * negative reverse). Otherwise, if the values are assumed to be unsigned
523  * (positive in both directions).
524  */
525  static constexpr uint8_t FLAG_SIGNED = 0x1;
526 
527  /** The time of the measurement, in P1 time (beginning at power-on). */
529 
530  /**
531  * The source of the incoming data, if known.
532  */
534 
535  /**
536  * The transmission gear currently in use, or direction of motion, if
537  * available.
538  */
540 
541  /** A bitmask of flags associated with the measurement data. */
542  uint8_t flags = 0x0;
543 
544  uint8_t reserved = 0;
545 
546  /** The front left wheel speed (in m/s). Set to NAN if not available. */
547  float front_left_speed_mps = NAN;
548 
549  /** The front right wheel speed (in m/s). Set to NAN if not available. */
550  float front_right_speed_mps = NAN;
551 
552  /** The rear left wheel speed (in m/s). Set to NAN if not available. */
553  float rear_left_speed_mps = NAN;
554 
555  /** The rear right wheel speed (in m/s). Set to NAN if not available. */
556  float rear_right_speed_mps = NAN;
557 };
558 
559 /**
560  * @brief Raw (uncorrected) dfferential wheel speed measurement output (@ref
561  * MessageType::RAW_WHEEL_SPEED_OUTPUT, version 1.0).
562  * @ingroup measurement_messages
563  *
564  * This message is an output from the device that contains the speed of each
565  * individual wheel on the vehicle. These measurements come directly from the
566  * sensor, and do not have any corrections or calibration applied.
567  *
568  * See @ref WheelSpeedOutput for more details. See also @ref WheelSpeedInput.
569  */
571  static constexpr MessageType MESSAGE_TYPE =
573  static constexpr uint8_t MESSAGE_VERSION = 0;
574 
575  /**
576  * Set this flag if the measured wheel speeds are signed (positive forward,
577  * negative reverse). Otherwise, if the values are assumed to be unsigned
578  * (positive in both directions).
579  */
580  static constexpr uint8_t FLAG_SIGNED = 0x1;
581 
582  /**
583  * Measurement timestamp and additional information, if available. See @ref
584  * MeasurementDetails for details.
585  */
587 
588  /**
589  * The front left wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
590  * available.
591  */
592  int32_t front_left_speed = INT32_MAX;
593 
594  /**
595  * The front right wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
596  * available.
597  */
598  int32_t front_right_speed = INT32_MAX;
599 
600  /**
601  * The rear left wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
602  * available.
603  */
604  int32_t rear_left_speed = INT32_MAX;
605 
606  /**
607  * The rear right wheel speed (in m/s * 2^-10). Set to 0x7FFFFFFF if not
608  * available.
609  */
610  int32_t rear_right_speed = INT32_MAX;
611 
612  /**
613  * The transmission gear currently in use, or direction of motion, if
614  * available.
615  *
616  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
617  * direction information is available externally.
618  */
620 
621  /** A bitmask of flags associated with the measurement data. */
622  uint8_t flags = 0x0;
623 
624  uint8_t reserved[2] = {0};
625 };
626 
627 ////////////////////////////////////////////////////////////////////////////////
628 // Vehicle Speed Measurements
629 ////////////////////////////////////////////////////////////////////////////////
630 
631 /**
632  * @brief Vehicle body speed measurement input (@ref
633  * MessageType::VEHICLE_SPEED_INPUT, version 1.0).
634  * @ingroup measurement_messages
635  *
636  * This message is an input to the device, used to convey the along-track speed
637  * of the vehicle (forward/backward). For differential speed measurements for
638  * multiple wheels, see @ref WheelSpeedInput.
639  *
640  * To use vehicle speed data, you must first configure the device by issuing a
641  * @ref SetConfigMessage message containing a @ref WheelConfig payload
642  * describing the vehicle sensor configuration (speed data signed/unsigned,
643  * etc.).
644  *
645  * Some platforms may have an additional voltage signal used to indicate
646  * direction of motion, have direction or gear information available from a
647  * vehicle CAN bus, etc. If direction/gear information is available, it may be
648  * provided in the @ref gear field.
649  *
650  * To send wheel tick counts from software, use @ref VehicleTickInput instead.
651  *
652  * See also @ref VehicleSpeedOutput for measurement output.
653  */
655  static constexpr MessageType MESSAGE_TYPE = MessageType::VEHICLE_SPEED_INPUT;
656  static constexpr uint8_t MESSAGE_VERSION = 0;
657 
658  /**
659  * Set this flag if the measured wheel speeds are signed (positive forward,
660  * negative reverse). Otherwise, if the values are assumed to be unsigned
661  * (positive in both directions).
662  */
663  static constexpr uint8_t FLAG_SIGNED = 0x1;
664 
665  /**
666  * Measurement timestamp and additional information, if available. See @ref
667  * MeasurementDetails for details.
668  */
670 
671  /**
672  * The current vehicle speed estimate (in m/s * 2^-10). Set to 0x7FFFFFFF if
673  * not available.
674  */
675  int32_t vehicle_speed = INT32_MAX;
676 
677  /**
678  * The transmission gear currently in use, or direction of motion, if
679  * available.
680  *
681  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
682  * direction information is available externally.
683  */
685 
686  /** A bitmask of flags associated with the measurement data. */
687  uint8_t flags = 0x0;
688 
689  uint8_t reserved[2] = {0};
690 };
691 
692 /**
693  * @brief Vehicle body speed measurement output with calibration and corrections
694  * applied (@ref MessageType::VEHICLE_SPEED_OUTPUT, version 1.0).
695  * @ingroup measurement_messages
696  *
697  * This message is an output from the device that contains the along-track speed
698  * of the vehicle (forward/backward), after applying any estimated corrections
699  * for scale factor, etc.
700  *
701  * Odometry data may be received via a software input from a host machine, a
702  * vehicle CAN bus, or a hardware voltage signal (wheel ticks). The @ref
703  * data_source field will indicate which type of data source provided the
704  * measurements to the device.
705  *
706  * @note
707  * When odometry is provided using hardware wheel ticks, the output rate of this
708  * message may differ from the wheel tick input rate. For high accuracy
709  * applications, FusionEngine may integrate tick counts over longer intervals to
710  * improve performance.
711  *
712  * See also @ref VehicleSpeedInput and @ref RawVehicleSpeedOutput.
713  */
715  static constexpr MessageType MESSAGE_TYPE = MessageType::VEHICLE_SPEED_OUTPUT;
716  static constexpr uint8_t MESSAGE_VERSION = 0;
717 
718  /**
719  * Set this flag if the measured wheel speeds are signed (positive forward,
720  * negative reverse). Otherwise, if the values are assumed to be unsigned
721  * (positive in both directions).
722  */
723  static constexpr uint8_t FLAG_SIGNED = 0x1;
724 
725  /** The time of the measurement, in P1 time (beginning at power-on). */
727 
728  /**
729  * The source of the incoming data, if known.
730  */
732 
733  /**
734  * The transmission gear currently in use, or direction of motion, if
735  * available.
736  *
737  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
738  * direction information is available externally.
739  */
741 
742  /** A bitmask of flags associated with the measurement data. */
743  uint8_t flags = 0x0;
744 
745  uint8_t reserved = 0;
746 
747  /** The current vehicle speed estimate (in m/s). */
748  float vehicle_speed_mps = NAN;
749 };
750 
751 /**
752  * @brief Raw (uncorrected) vehicle body speed measurement output (@ref
753  * MessageType::RAW_VEHICLE_SPEED_OUTPUT, version 1.0).
754  * @ingroup measurement_messages
755  *
756  * This message is an output from the device that contains the along-track speed
757  * of the vehicle (forward/backward). These measurements come directly from the
758  * sensor, and do not have any corrections or calibration applied.
759  *
760  * See @ref VehicleSpeedOutput for more details. See also @ref
761  * VehicleSpeedInput.
762  */
764  static constexpr MessageType MESSAGE_TYPE =
766  static constexpr uint8_t MESSAGE_VERSION = 0;
767 
768  /**
769  * Set this flag if the measured wheel speeds are signed (positive forward,
770  * negative reverse). Otherwise, if the values are assumed to be unsigned
771  * (positive in both directions).
772  */
773  static constexpr uint8_t FLAG_SIGNED = 0x1;
774 
775  /**
776  * Measurement timestamp and additional information, if available. See @ref
777  * MeasurementDetails for details.
778  */
780 
781  /**
782  * The current vehicle speed estimate (in m/s * 2^-10). Set to 0x7FFFFFFF if
783  * not available.
784  */
785  int32_t vehicle_speed = INT32_MAX;
786 
787  /**
788  * The transmission gear currently in use, or direction of motion, if
789  * available.
790  *
791  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
792  * direction information is available externally.
793  */
795 
796  /** A bitmask of flags associated with the measurement data. */
797  uint8_t flags = 0x0;
798 
799  uint8_t reserved[2] = {0};
800 };
801 
802 ////////////////////////////////////////////////////////////////////////////////
803 // Wheel Tick Measurements
804 ////////////////////////////////////////////////////////////////////////////////
805 
806 /**
807  * @brief Differential wheel encoder tick input (@ref
808  * MessageType::WHEEL_TICK_INPUT, version 1.0).
809  * @ingroup measurement_messages
810  *
811  * This message is an input to the device, used to convey the wheel encoder tick
812  * counts for one or more wheels, received through software (e.g., vehicle CAN
813  * bus) or captured in hardware from external voltage pulses. The number and
814  * type of wheels expected, and the interpretation of the tick count values,
815  * varies by vehicle.
816  *
817  * To use wheel encoder data, you must first configure the device by issuing a
818  * @ref SetConfigMessage message containing a @ref WheelConfig payload
819  * describing the vehicle sensor configuration (tick counts signed/unsigned,
820  * etc.).
821  *
822  * Some platforms may have an additional voltage signal used to indicate
823  * direction of motion, have direction or gear information available from a
824  * vehicle CAN bus, etc. If direction/gear information is available, it may be
825  * provided in the @ref gear field.
826  *
827  * See also @ref RawWheelTickOutput for measurement output.
828  */
830  static constexpr MessageType MESSAGE_TYPE = MessageType::WHEEL_TICK_INPUT;
831  static constexpr uint8_t MESSAGE_VERSION = 0;
832 
833  /**
834  * Measurement timestamp and additional information, if available. See @ref
835  * MeasurementDetails for details.
836  */
838 
839  /**
840  * The front left wheel ticks. The interpretation of these ticks is
841  * defined outside of this message.
842  */
843  uint32_t front_left_wheel_ticks = 0;
844 
845  /**
846  * The front right wheel ticks. The interpretation of these ticks is
847  * defined outside of this message.
848  */
849  uint32_t front_right_wheel_ticks = 0;
850 
851  /**
852  * The rear left wheel ticks. The interpretation of these ticks is
853  * defined outside of this message.
854  */
855  uint32_t rear_left_wheel_ticks = 0;
856 
857  /**
858  * The rear right wheel ticks. The interpretation of these ticks is
859  * defined outside of this message.
860  */
861  uint32_t rear_right_wheel_ticks = 0;
862 
863  /**
864  * The transmission gear currently in use, or direction of motion, if
865  * available.
866  *
867  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
868  * direction information is available externally.
869  */
871 
872  uint8_t reserved[3] = {0};
873 };
874 
875 /**
876  * @brief Raw (uncorrected) dfferential wheel encoder tick output (@ref
877  * MessageType::RAW_WHEEL_TICK_OUTPUT, version 1.0).
878  * @ingroup measurement_messages
879  *
880  * This message is an output from the device that contains wheel encoder tick
881  * counts for each individual wheel on the vehicle. These measurements come
882  * directly from the sensor, and do not have any corrections or calibration
883  * applied.
884  *
885  * See also @ref WheelTickInput.
886  */
888  static constexpr MessageType MESSAGE_TYPE =
890  static constexpr uint8_t MESSAGE_VERSION = 0;
891 
892  /**
893  * Measurement timestamp and additional information, if available. See @ref
894  * MeasurementDetails for details.
895  */
897 
898  /**
899  * The front left wheel ticks. The interpretation of these ticks is
900  * defined outside of this message.
901  */
902  uint32_t front_left_wheel_ticks = 0;
903 
904  /**
905  * The front right wheel ticks. The interpretation of these ticks is
906  * defined outside of this message.
907  */
908  uint32_t front_right_wheel_ticks = 0;
909 
910  /**
911  * The rear left wheel ticks. The interpretation of these ticks is
912  * defined outside of this message.
913  */
914  uint32_t rear_left_wheel_ticks = 0;
915 
916  /**
917  * The rear right wheel ticks. The interpretation of these ticks is
918  * defined outside of this message.
919  */
920  uint32_t rear_right_wheel_ticks = 0;
921 
922  /**
923  * The transmission gear currently in use, or direction of motion, if
924  * available.
925  *
926  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
927  * direction information is available externally.
928  */
930 
931  uint8_t reserved[3] = {0};
932 };
933 
934 ////////////////////////////////////////////////////////////////////////////////
935 // Vehicle Tick Measurements
936 ////////////////////////////////////////////////////////////////////////////////
937 
938 /**
939  * @brief Single wheel encoder tick input, representing vehicle body speed
940  * (@ref MessageType::VEHICLE_TICK_INPUT, version 1.0).
941  * @ingroup measurement_messages
942  *
943  * This message is an input to the device, used to convey a single wheel encoder
944  * tick count representing the along-track speed of the vehicle
945  * (forward/backward). Tick data may be received through software (e.g., vehicle
946  * CAN bus) or captured in hardware from external voltage pulses.The
947  * interpretation of the tick count values varies by vehicle.
948  *
949  * To use wheel encoder data, you must first configure the device by issuing a
950  * @ref SetConfigMessage message containing either a @ref WheelConfig payload
951  * (software source) or @ref HardwareTickConfig payload (hardware voltage)
952  * describing the vehicle sensor configuration (tick counts signed/unsigned,
953  * etc.).
954  *
955  * Some platforms may have an additional voltage signal used to indicate
956  * direction of motion, have direction or gear information available from a
957  * vehicle CAN bus, etc. If direction/gear information is available, it may be
958  * provided in the @ref gear field.
959  *
960  * See also @ref RawVehicleTickOutput for measurement output.
961  */
963  static constexpr MessageType MESSAGE_TYPE = MessageType::VEHICLE_TICK_INPUT;
964  static constexpr uint8_t MESSAGE_VERSION = 0;
965 
966  /**
967  * Measurement timestamp and additional information, if available. See @ref
968  * MeasurementDetails for details.
969  */
971 
972  /**
973  * The current encoder tick count. The interpretation of these ticks is
974  * defined outside of this message.
975  */
976  uint32_t tick_count = 0;
977 
978  /**
979  * The transmission gear currently in use, or direction of motion, if
980  * available.
981  *
982  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
983  * direction information is available externally.
984  */
986 
987  uint8_t reserved[3] = {0};
988 };
989 
990 /**
991  * @brief Raw (uncorrected) single wheel encoder tick output (@ref
992  * MessageType::RAW_VEHICLE_TICK_OUTPUT, version 1.0).
993  * @ingroup measurement_messages
994  *
995  * This message is an output from the device that contains a wheel encoder tick
996  * count representing the along-track speed of the vehicle (forward/backward).
997  * This value comes directly from the sensor, and does not have any corrections
998  * or calibration applied.
999  *
1000  * See also @ref VehicleTickInput.
1001  */
1003  static constexpr MessageType MESSAGE_TYPE =
1005  static constexpr uint8_t MESSAGE_VERSION = 0;
1006 
1007  /**
1008  * Measurement timestamp and additional information, if available. See @ref
1009  * MeasurementDetails for details.
1010  */
1012 
1013  /**
1014  * The current encoder tick count. The interpretation of these ticks is
1015  * defined outside of this message.
1016  */
1017  uint32_t tick_count = 0;
1018 
1019  /**
1020  * The transmission gear currently in use, or direction of motion, if
1021  * available.
1022  *
1023  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
1024  * direction information is available externally.
1025  */
1027 
1028  uint8_t reserved[3] = {0};
1029 };
1030 
1031 ////////////////////////////////////////////////////////////////////////////////
1032 // Deprecated Speed Measurement Definitions
1033 ////////////////////////////////////////////////////////////////////////////////
1034 
1035 /**
1036  * @brief (Deprecated) Differential wheel speed measurement (@ref
1037  * MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT, version 1.0).
1038  * @ingroup measurement_messages
1039  *
1040  * @deprecated
1041  * This message is deprecated as of version 1.18.0 and may be removed in the
1042  * future. It should not used for new development. See @ref WheelSpeedInput and
1043  * @ref WheelSpeedOutput instead.
1044  */
1046  static constexpr MessageType MESSAGE_TYPE =
1048  static constexpr uint8_t MESSAGE_VERSION = 0;
1049 
1050  /**
1051  * Measurement timestamp and additional information, if available. See @ref
1052  * MeasurementDetails for details.
1053  */
1055 
1056  /** The front left wheel speed (in m/s). Set to NAN if not available. */
1057  float front_left_speed_mps = NAN;
1058 
1059  /** The front right wheel speed (in m/s). Set to NAN if not available. */
1060  float front_right_speed_mps = NAN;
1061 
1062  /** The rear left wheel speed (in m/s). Set to NAN if not available. */
1063  float rear_left_speed_mps = NAN;
1064 
1065  /** The rear right wheel speed (in m/s). Set to NAN if not available. */
1066  float rear_right_speed_mps = NAN;
1067 
1068  /**
1069  * The transmission gear currently in use, or direction of motion, if
1070  * available.
1071  *
1072  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
1073  * direction information is available externally.
1074  */
1076 
1077  /**
1078  * `true` if the wheel speeds are signed (positive forward, negative reverse),
1079  * or `false` if the values are unsigned (positive in both directions).
1080  */
1081  bool is_signed = true;
1082 
1083  uint8_t reserved[2] = {0};
1084 };
1085 
1086 /**
1087  * @brief (Deprecated) Vehicle body speed measurement (@ref
1088  * MessageType::DEPRECATED_VEHICLE_SPEED_MEASUREMENT, version 1.0).
1089  * @ingroup measurement_messages
1090  *
1091  * @deprecated
1092  * This message is deprecated as of version 1.18.0 and may be removed in the
1093  * future. It should not used for new development. See @ref VehicleSpeedInput
1094  * and @ref VehicleSpeedOutput instead.
1095  */
1097  static constexpr MessageType MESSAGE_TYPE =
1099  static constexpr uint8_t MESSAGE_VERSION = 0;
1100 
1101  /**
1102  * Measurement timestamp and additional information, if available. See @ref
1103  * MeasurementDetails for details.
1104  */
1106 
1107  /** The current vehicle speed estimate (in m/s). */
1108  float vehicle_speed_mps = NAN;
1109 
1110  /**
1111  * The transmission gear currently in use, or direction of motion, if
1112  * available.
1113  *
1114  * Set to @ref GearType::FORWARD or @ref GearType::REVERSE where vehicle
1115  * direction information is available externally.
1116  */
1118 
1119  /**
1120  * `true` if the wheel speeds are signed (positive forward, negative reverse),
1121  * or `false` if the values are unsigned (positive in both directions).
1122  */
1123  bool is_signed = true;
1124 
1125  uint8_t reserved[2] = {0};
1126 };
1127 
1128 ////////////////////////////////////////////////////////////////////////////////
1129 // Heading Sensor Definitions
1130 ////////////////////////////////////////////////////////////////////////////////
1131 
1132 /**
1133  * @brief Heading sensor measurement output with heading bias corrections
1134  * applied (@ref MessageType::HEADING_OUTPUT, version 1.0).
1135  * @ingroup measurement_messages
1136  *
1137  * This message is an output from the device contaning heading sensor
1138  * measurements after applying user-specified horizontal and vertical bias
1139  * corrections to account for the orientation of the primary and secondary GNSS
1140  * antennas.
1141  *
1142  * See also @ref RawHeadingOutput.
1143  */
1145  static constexpr MessageType MESSAGE_TYPE = MessageType::HEADING_OUTPUT;
1146  static constexpr uint8_t MESSAGE_VERSION = 0;
1147 
1148  /**
1149  * Measurement timestamp and additional information, if available. See @ref
1150  * MeasurementDetails for details.
1151  */
1153 
1154  /**
1155  * Set to @ref SolutionType::RTKFixed when heading is available, or @ref
1156  * SolutionType::Invalid otherwise.
1157  */
1159 
1160  uint8_t reserved[3] = {0};
1161 
1162  /** A bitmask of flags associated with the solution. */
1163  uint32_t flags = 0;
1164 
1165  /**
1166  * The measured YPR vector (in degrees), resolved in the ENU frame.
1167  *
1168  * YPR is defined as an intrinsic Euler-321 rotation, i.e., yaw, pitch, then
1169  * roll.
1170  *
1171  * @note
1172  * This field contains the measured attitude information (@ref
1173  * RawHeadingOutput) from a secondary heading device after applying @ref
1174  * ConfigType::HEADING_BIAS configuration settings for yaw (horizontal) and
1175  * pitch (vertical) offsets between the primary and secondary GNSS antennas.
1176  * If either bias value is not specified, the corresponding measurement values
1177  * will be set to `NAN`.
1178  */
1179  float ypr_deg[3] = {NAN, NAN, NAN};
1180 
1181  /**
1182  * The heading angle (in degrees) with respect to true north, pointing from
1183  * the primary antenna to the secondary antenna, after applying bias
1184  * corrections.
1185  *
1186  * @note
1187  * Reported in the range [0, 360).
1188  */
1189  float heading_true_north_deg = NAN;
1190 };
1191 
1192 /**
1193  * @brief Raw (uncorrected) heading sensor measurement output (@ref
1194  * MessageType::RAW_HEADING_OUTPUT, version 1.0).
1195  * @ingroup measurement_messages
1196  *
1197  * This message is an output from the device contaning raw heading sensor
1198  * measurements that have not been corrected for mounting angle biases.
1199  *
1200  * See also @ref HeadingOutput.
1201  */
1203  static constexpr MessageType MESSAGE_TYPE = MessageType::RAW_HEADING_OUTPUT;
1204  static constexpr uint8_t MESSAGE_VERSION = 0;
1205 
1206  /**
1207  * Measurement timestamp and additional information, if available. See @ref
1208  * MeasurementDetails for details.
1209  */
1211 
1212  /**
1213  * Set to @ref SolutionType::RTKFixed when heading is available, or @ref
1214  * SolutionType::Invalid otherwise.
1215  */
1217 
1218  uint8_t reserved[3] = {0};
1219 
1220  /** A bitmask of flags associated with the solution. */
1221  uint32_t flags = 0;
1222 
1223  /**
1224  * The position of the secondary GNSS antenna relative to the primary antenna
1225  * (in meters), resolved with respect to the local ENU tangent plane: east,
1226  * north, up.
1227  *
1228  * Position is measured with respect to the primary antenna as follows:
1229  * @f[
1230  * \Delta r_{ENU} = C^{ENU}_{ECEF} (r_{Secondary, ECEF} - r_{Primary, ECEF})
1231  * @f]
1232  */
1233  float relative_position_enu_m[3] = {NAN, NAN, NAN};
1234 
1235  /**
1236  * The position standard deviation (in meters), resolved with respect to the
1237  * local ENU tangent plane: east, north, up.
1238  */
1239  float position_std_enu_m[3] = {NAN, NAN, NAN};
1240 
1241  /**
1242  * The measured heading angle (in degrees) with respect to true north,
1243  * pointing from the primary antenna to the secondary antenna.
1244  *
1245  * @note
1246  * Reported in the range [0, 360).
1247  */
1248  float heading_true_north_deg = NAN;
1249 
1250  /**
1251  * The estimated distance between primary and secondary antennas (in meters).
1252  */
1253  float baseline_distance_m = NAN;
1254 };
1255 
1256 #pragma pack(pop)
1257 
1258 } // namespace messages
1259 } // namespace fusion_engine
1260 } // namespace point_one
@ RAW_VEHICLE_SPEED_OUTPUT
RawVehicleSpeedOutput
@ GPS_TIME
Message timestamped in GPS time, referenced to 1980/1/6.
@ CAN
Sensor data captured from a vehicle CAN bus.
SystemTimeSource
The source of a point_one::fusion_engine::messages::Timestamp used to represent the time of applicabi...
Definition: measurements.h:97
@ IMU_INPUT
IMUInput
Raw (uncorrected) dfferential wheel encoder tick output (MessageType::RAW_WHEEL_TICK_OUTPUT,...
Definition: measurements.h:887
(Deprecated) Differential wheel speed measurement (MessageType::DEPRECATED_WHEEL_SPEED_MEASUREMENT,...
Differential wheel encoder tick input (MessageType::WHEEL_TICK_INPUT, version 1.0).
Definition: measurements.h:829
MessageType
Identifiers for the defined output message types.
Definition: defs.h:34
Library portability helper definitions.
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:252
Raw (uncorrected) single wheel encoder tick output (MessageType::RAW_VEHICLE_TICK_OUTPUT,...
@ INVALID
Timestamp not valid.
Single wheel encoder tick input, representing vehicle body speed (MessageType::VEHICLE_TICK_INPUT,...
Definition: measurements.h:962
SolutionType
Navigation solution type definitions.
Definition: defs.h:464
Timestamp measurement_time
The measurement time of applicability, if available, in a user-specified time base.
Definition: measurements.h:200
#define P1_ALIGNAS(N)
Definition: portability.h:57
@ RAW_VEHICLE_TICK_OUTPUT
RawVehicleTickOutput
@ DEPRECATED_WHEEL_SPEED_MEASUREMENT
DeprecatedWheelSpeedMeasurement
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:896
Raw (uncorrected) vehicle body speed measurement output (MessageType::RAW_VEHICLE_SPEED_OUTPUT,...
Definition: measurements.h:763
@ SERIAL
Sensor data provided over a serial connection.
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:337
@ RAW_WHEEL_SPEED_OUTPUT
RawWheelSpeedOutput
@ TIMESTAMPED_ON_RECEPTION
Message timestamped in system time, generated when received by the device.
@ HEADING_OUTPUT
HeadingOutput
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:586
@ NEUTRAL
The vehicle is in neutral.
Raw (uncorrected) dfferential wheel speed measurement output (MessageType::RAW_WHEEL_SPEED_OUTPUT,...
Definition: measurements.h:570
MeasurementDetails details
Measurement timestamp and additional information, if available.
Timestamp p1_time
The P1 time corresponding with the measurement time of applicability, if available.
Definition: measurements.h:227
@ DEPRECATED_VEHICLE_SPEED_MEASUREMENT
DeprecatedVehicleSpeedMeasurement
The base class for all message payloads.
Definition: defs.h:648
Raw (uncorrected) IMU sensor measurement output (MessageType::RAW_IMU_OUTPUT, version 1....
Definition: measurements.h:329
SensorDataSource
The source of received sensor measurements, if known.
Definition: measurements.h:33
@ SENDER_SYSTEM_TIME
Message timestamp was generated from a monotonic clock of an external system.
MeasurementDetails details
Measurement timestamp and additional information, if available.
@ REVERSE
The vehicle is in reverse.
Differential wheel speed measurement input (MessageType::WHEEL_SPEED_INPUT, version 1....
Definition: measurements.h:437
@ Invalid
Invalid, no position available.
GNSS signal and frequency type definitions.
Definition: logging.h:38
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:779
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:669
@ IMU_OUTPUT
IMUOutput
Timestamp p1_time
The time of the measurement, in P1 time (beginning at power-on).
Definition: measurements.h:291
@ RAW_IMU_OUTPUT
RawIMUOutput
P1_CONSTEXPR_FUNC const char * to_string(ConfigType type)
Get a human-friendly string name for the specified ConfigType.
GearType
The current transmission gear used by the vehicle.
Definition: measurements.h:367
Heading sensor measurement output with heading bias corrections applied (MessageType::HEADING_OUTPUT,...
std::ostream p1_ostream
Definition: portability.h:75
IMU sensor measurement output with calibration and corrections applied (MessageType::IMU_OUTPUT,...
Definition: measurements.h:286
p1_ostream & operator<<(p1_ostream &stream, ConfigType type)
ConfigType stream operator.
@ VEHICLE_SPEED_INPUT
VehicleSpeedInput
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:452
The time of applicability and additional information for an incoming sensor measurement.
Definition: measurements.h:193
@ NETWORK
Sensor data provided over a network connection.
@ WHEEL_SPEED_INPUT
WheelSpeedInput
@ UNKNOWN
Data source not known.
@ WHEEL_SPEED_OUTPUT
WheelSpeedOutput
@ RAW_WHEEL_TICK_OUTPUT
RawWheelTickOutput
#define P1_CONSTEXPR_FUNC
Definition: portability.h:105
@ INTERNAL
Sensor data captured internal to the device (embedded IMU, GNSS receiver, etc.).
@ VEHICLE_SPEED_OUTPUT
VehicleSpeedOutput
@ VEHICLE_TICK_INPUT
VehicleTickInput
Timestamp p1_time
The time of the measurement, in P1 time (beginning at power-on).
Definition: measurements.h:726
Raw (uncorrected) heading sensor measurement output (MessageType::RAW_HEADING_OUTPUT,...
MeasurementDetails details
Measurement timestamp and additional information, if available.
MeasurementDetails details
Measurement timestamp and additional information, if available.
Timestamp p1_time
The time of the measurement, in P1 time (beginning at power-on).
Definition: measurements.h:528
(Deprecated) Vehicle body speed measurement (MessageType::DEPRECATED_VEHICLE_SPEED_MEASUREMENT,...
@ P1_TIME
Message timestamped in P1 time.
Vehicle body speed measurement input (MessageType::VEHICLE_SPEED_INPUT, version 1....
Definition: measurements.h:654
@ FORWARD
The vehicle is in a forward gear.
Point One FusionEngine output message common definitions.
@ PARK
The vehicle is parked.
Generic timestamp representation.
Definition: defs.h:549
Vehicle body speed measurement output with calibration and corrections applied (MessageType::VEHICLE_...
Definition: measurements.h:714
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:970
@ HARDWARE_IO
Sensor data generated via hardware voltage signal (wheel tick, external event, etc....
@ UNKNOWN
The transmission gear is not known, or does not map to a supported GearType.
@ RAW_HEADING_OUTPUT
RawHeadingOutput
Differential wheel speed measurement output with calibration and corrections applied (MessageType::WH...
Definition: measurements.h:516
MeasurementDetails details
Measurement timestamp and additional information, if available.
Definition: measurements.h:837
@ WHEEL_TICK_INPUT
WheelTickInput
IMU sensor measurement input (MessageType::IMU_INPUT, version 1.0).
Definition: measurements.h:244
MeasurementDetails details
Measurement timestamp and additional information, if available.