Create a Dynamic Driver Interface

Every Dynamic Driver interface is different. To help you navigate your particular interface, this article outlines what we consider to be the best practices for creating a Dynamic Driver interface.

PolySync provides an open infrastructure to create a dynamic driver interface for any hardware device. All hardware, whether it’s for perception or actuation, communicates with the PolySync bus through an interface to the PolySync Dynamic Driver .

1. The test interface

The test interface acts to exercise the Hardware Abstraction Layer (HAL) and ensure the sensor interface specification isn’t being misread, or that it contains any bugs.

The PolySync Abstraction Layer (PAL) handles polling for data, timestamping, transforms, record and replay, abstraction, publishing, and more.

This workflow is coupled with the dynamic driver interface source code (below) for the Phidget Spatial Accelerometer. To create a supported sensor interface, it’s required that the interface follows the template provided in the Phidget Spatial example.

Get the Phidget Spatial Dynamic Driver interface source code from the PolySync C example Github repo.

Here’s a tree view of the Phidget Spatial HAL files:

drivers/phidget_spatial/
├── include
│   └── phidget_spatial_driver.h
├── Makefile
└── src
    ├── phidget_spatial_driver.c
    └── phidget_spatial_test_interface.c

Here’s a tree view of the Phidget Spatial PAL files:

interfaces/phidget_spatial/
├── include
│   ├── connect.h
│   ├── debug.h
│   ├── diagnostics.h
│   ├── getset.h
│   ├── interface_data.h
│   ├── log.h
│   ├── options.h
│   ├── process.h
│   ├── read.h
│   ├── sdf.h
│   └── type_support.h
├── Makefile
└── src
    ├── connect.c
    ├── debug.c
    ├── diagnostics.c
    ├── getset.c
    ├── interface.c
    ├── log.c
    ├── options.c
    ├── process.c
    ├── read.c
    ├── sdf.c
    └── type_support.c

2. Sensor interface specification

The communication protocol and hardware specification will detail, among other things:

  • What native interface(s) the device communicates on (see note below on multiple native interfaces)
  • Ethernet device will use the PolySync Socket API to open and close a socket for TCP or UDP communication
  • CAN based devices will use the PolySync CAN API
  • Serial and USB devices will use the PolySync Serial API to open and close a serial connection

  • Communication protocol

  • Off-the-wire: Binary interface, typically with a custom protocol written and provided by the hardware manufacturer; must parse and dissect the bytes from the wire

  • Software API: Software API written to provide easier access to sensor data through function calls

  • The protocol method of communication on the native interface

  • Does the device and/or data need to be validated?

  • Is it a polling interface or is the data requested?

  • How many native interfaces are required to send/receive all data?

  • What data is provided by the sensor

  • What data does the user want to parse from the sensor and publish to the PolySync bus?

  • Is there a PolySync message in the data model that can hold all the sensor data?

  • Are there runtime parameters that need get/set support for?

  • Are there native timestamps available?

  • For actuators, the command input structure

  • How are commands interpreted by the actuator interface?

  • Mounting, configuration, and installation requirements [/expanded]

Any sensor can connect and communicate on multiple native interfaces if the Dynamic Driver sensor interface requires it. A good example is the Ibeo LUX 4L.

The Lux offers LiDAR point and object data over a direct private Ethernet connection, and expects vehicle ego-motion data─velocity and yaw rate─over a separate private CAN bus. The Neobotix Ultrasonic Dynamic Driver interface communicates with sixteen individual sensors through a single CAN channel.

2.1.1 Phidget Spatial

The Phidget Spatial uses the Phidget Common API to open and close the hardware handle for the sensor through a serial USB connection. There is a specific Phidget Spatial API that is used to configure the sensor and provides two methods to access the accelerometer data:

  • Request the axes acceleration value
  • Asynchronously process the axes acceleration through a callback function

You can find documentation for the Phidget Spatial on the website.

  • Phidget Common API
  • Modules -> Phidget Common
  • Phidget Spatial API
  • Modules -> Specific Phidgets -> Phidget Spatial

3. Create requirements for the interface

As we all know, great software comes from great requirements. Using the questions from the previous section as a guide, create a set of requirements that meet the guidelines below:

  • Specific
  • Measurable
  • Attainable
  • Realizable
  • Time-bounded
  • Testable
  • Traceable

3.1 Creating a new message type

Part of writing requirements is determining which PolySync message the data will be published to the bus with. Check the existing data model and determine if a message type exists to store the sensor’s payload, the actuator’s command/response, or whatever else is needed.

If a suitable match cannot be found, you can create a new message type.

3.1.1 Phidget Spatial

Using the Phidget API documentation─in this case supplementing the communication protocol─and the answers to the questions from the previous step, you can create a set of requirements for the Phidget Spatial Dynamic Driver interface.

Looking at the existing data-model, you can see that there is a ps_platform_motion_msg that contains all the data fields that are provided by this sensor.

You can also see that there is a ‘ps_imu_msg’. It’s up to you to decide what message type(s) will be published to the bus at runtime. It is possible to have the interface populate and publish both message types.

4. Hardware Abstraction Layer

`phidget_spatial_driver.c`

The Hardware Abstraction Layer (HAL) is written in parallel with the test interface.

The HAL contains one or more functions to facilitate opening and closing the hardware device handle, for connecting, for configuring, and for parsing the various data types that are available.

The specific steps in this section will vary depending on the device protocol, but each HAL should be able fulfill these high level functions.

4.1 Open the native interface

Using the appropriate Device API, integrate the open and close features for the native interface into the Dynamic Driver interface.

PolySync provides the Device APIs for a PolySync node to connect to the raw byte stream or CAN frame stream, as well as an example on how to exercise the API.

4.1.1 Phidget Spatial

Within the Phidget Spatial interface, the axes count, min/max values for the reporting axes, and min/max data rate values are all defined as macro variables. An enum structure is used to represent the array index parameters.

Since Phidget provides an API to access the Spatial sensors data, the Phidget Common API will be used to open and close the native USB serial interface. The Spatial HAL contains a function for each of these two actions:

  • phidget_spatial_open_device()
  • phidget_spatial_close_device()

After referencing the Phidget Common API documentation, you can flush out the function body for these two routines.

4.2 Configure the device handle

Once a handle to the hardware device has been created in the open function, you can configure the handle for different operating modes, data rates, and other runtime parameters. This is all managed through the configure device function call.

4.2.1 Phidget Spatial

There is not a configuration function for the Spatial sensor. The following functions supplement the need for configuration function in the HAL.

  • phidget_spatial_get_data_rate()
  • phidget_spatial_set_data_rate()

Since you have the ability to modify the data rate at runtime, the PAL will be able to set the data rate based on the configuration parameter within the SDF Configurator.

4.3 Requesting data

The HAL needs to provide access to a sensor’s data payload. There are typically two methods:

  • Poll for data through individual function calls
  • Asynchronously parse sensor data to be stored in a local data structure

The HAL is only responsible for providing access to the data and should be as simple as possible.

The HAL does not need to worry about the state machine─the PAL will be able to handle polling for data. If a single read is not enough to fill a message, then the PAL will handle the state transitions to call the HALs read function multiple times. Keep the HAL simple.

4.3.1 Phidget Spatial

The Phidget HAL contains three functions to parse a sensor’s data. Each calls the respective functions for each of the three axes, and performs error checking.

  • phidget_spatial_get_acceleration()
  • Calls the Phidget Spatial API CPhidgetSpatial_getAcceleration()
  • phidget_spatial_get_angular_rate()
  • Calls the Phidget Spatial API CPhidgetSpatial_getAngularRate()
  • phidget_spatial_get_magnetic_field()
  • Calls the Phidget Spatial API CPhidgetSpatial_getMagneticField()

For parsing the axes data for acceleration, angular rate, and magnetic field, the functions provide pointers to double parameters for each axis (x,y, and z). Note the use of individual parameters for each axis rather than an array of doubles.

4.4 Tests

Begin to emphasize the use of integration, unit, and regression test framework. Start to move away from having a ‘test_interface’.

5. Test interface

phidget_spatial_test_interface.c

The test interface is written in parallel with the HAL.

A complete test interface must be able to open/close, connect, configure, and begin basic communication with the sensor or hardware device.

You can think of the test interface like the Hello, world! example. The test interface is what the you use while creating the HAL to perform the open/close, connect, configure, and parse some of the data.

Depending on the sensor, and the scope of the test interface, you could parse a single message from the sensor and exit, or continue to parse data until the interrupt signal is encountered.

Keep in mind that the main goal of the test interface is to ensure that you can successfully connect, configure, parse, validate, and close.

5.1 Open and close the native interface

The application typically executes directly from the main entry point. Once the HAL contains the function to open the device handle through the native interface, the test interface should call the open function.

Once the open function is working properly, you should immediately get the reciprocal close function implemented. Without properly closing the hardware handle, you should expect to see odd and indeterminate behaviour during testing.

5.2 Configure the device handle

The test interface should select a known good configuration, usually the default configuration defined by the OEM, and configure the device for parsing data. For some devices configuration may not be necessary─simply calling open from the HAL initializes the device.

5.2.1 Phidget Spatial

There are no required configurations for the Phidget Spatial. The Phidget Spatial allows you to set the device data rate. Since the device does not enforce that the configuration is set before reading data, the get and set data rate features were implemented as functions that could be called anytime.

5.3 Requesting data

It is up to you to determine which data to request and print within the test interface. To ease debugging in the field, the test interface should request and print the data that will eventually be published to the PolySync bus.

This is also useful during PAL development, allowing you to compare the data as it was on the wire, and the data represented in the abstracted PolySync message.

5.3.1 Phidget Spatial

The Phidget Spatial test interface continuously calls the three get functions to request and print the acceleration, angular rate, and magnetic field strength.

6. PolySync Abstraction Layer (PAL)

The PAL is the workhorse of the PolySync Dynamic Driver interface. The PAL interfaces directly with the Dynamic Driver application through pre-defined functions, which are exposed to the Dynamic Driver at runtime.

When the PAL is finished, there will be a *.so shared object library that is compatible with the SDF Configurator and the Dynamic Driver. It is used to expose the functionality of the PAL to the upper PolySync layers.

A PAL leverages the HAL to request sensor data as it cycles through the Dynamic Driver state machine. The state machine is managed by the top-level Dynamic Driver application, which will call each of the functions defined in the PAL.

Every PAL contains some file names and function names that must follow the PolySync naming convention. They are detailed below:

  • Expected Expose Functions

    • psync_interface_expose_options
    • psync_interface_expose_parameters
    • psync_interface_expose_sdf_support
    • psync_interface_expose_sensor_support
    • psync_interface_expose_type_support
  • Expected Functions in the State machine, given in the order they are called

    • psync_interface_configure
    • psync_interface_init
    • psync_interface_poll_for_data
    • psync_interface_process_data
    • psync_interface_log_data
    • psync_interface_idle
    • psync_interface_handle_command [optional]
    • psync_interface_handle_parameter_command
    • psync_interface_handle_state
    • psync_interface_release

While in an OK state, a node will loop between psync_interface_poll_for_data and psync_interface_handle_state as fast as computationally possible. The order in which it loops is:

poll -> process -> idle -> poll -> process -> idle, etc.

And it only enters handle_state when a state other than OK is entered. It stays inside of handle_state until psync_node_recover_fault is called.

It’s very important to note that a Dynamic Driver interface PAL does not perform any algorithmic computation on the sensor data. It is abstracted into a PolySync message, and published to the bus. To maintain the integrity of recorded data through replay in future sessions, the interface PAL should focus on abstraction and not algorithmic processing.

6.1 Concepts

The following are core concepts that should be well understood before continuing with development of the PAL.

6.1.1 Abstraction

The sensor data abstraction occurs during the psync_interface_process_data function. The PALs job is to transcribe data from the HAL and place it into a PolySync message type that is defined within the PolySync data model. Part of writing requirements is determining which message the data will be published to the bus with.

If a new message type must be created, now is the time to devote effort.

6.2 Control flow

It’s very important to leave the control flow of the interface to the dynamic driver. Do not try and override the state machine to force states. The interface should return Diagnostic Trouble Codes (DTC) to indicate error states and let the Dynamic Driver state machine handle state changes.

When it’s appropriate, the top-level dynamic driver calls the interfaces handle_state function, which allows the PAL to attempt to recover from a DTC.

6.2.1 Expose options

The exposed data from the PAL is expected to be a constant─statically initialized data set─used to provide a baseline configuration model that the SDF Configurator and Dynamic Driver use to verify and/or update the installed SDF’s data.

6.2.2 Handling faults

The PolySync Dynamic Driver interface must be able to dynamically respond to any errors that are encountered. When a fault is detected, the interface should be set up to:

  • Close, open, and reconfigure the hardware handle
  • Validate the sensor data
  • Enter the OK state and resume the normal flow of execution

Faults can range from configuration and usage errors, to sensor fault states being triggered, and to issues creating a connection to the native interface. All faults are represented with a Diagnostic Trouble Code (DTC).

6.2.3 Handling current DTC state

Within the psync_interface_handle_state, the PAL should check the dynamic_driver -> current_state -> dtc and perform the appropriate action for the DTC present.

For example if DTC_NONATIVEBUS is encountered, the PAL should close the connection, re-open it, configure, validate, etc.

6.3 Post-Processing data

Interfaces do not post-process sensor data. The interface is responsible for reading data from the wire and abstracting the data into a PolySync message. Any data processing such as object detection, image recognition, machine learning, etc. is not performed with the interface.

6.3.1 Publishing messages

It can happen wherever the PAL sees fit. Most of PAL’s do their publishing in the process data section. This includes publishing the raw/low-level messages, if enabled.

6.3.2 Sending data to the device

Some sensors and hardware devices require reference data (such as vehicle ego-motion) to perform post-processing on the data before it is sent on the native interface to the Dynamic Driver interface. An interface can subscribe to as many message types as necessary to fulfill this sensor requirement, though typically, only platform-motion data is required.

6.3.3 Timestamping

The PolySync timestamps are applied to the message by the PAL before the message is published to the bus. There are multiple timestamp fields that must be carefully managed.

Most of the Rx timestamps are provided by the lower level API’s, like Socket/CAN/Serial. These Rx timestamps typically get copied into the published messages message.timestamp field if it exists.

  • Message Header Timestamp
    • message -> header.timestamp
    • Always represents the time that this message was published to the bus
    • During replay the Message Header Timestamp still represents when the message was published to the bus, today’s UTC timestamp
  • Message Timestamp
    • message.timestamp
    • Always represents when the message was received by the PolySync Dynamic Driver interface, over the native interface
    • During replay this timestamp will contain the UTC timestamp of the day of recording
    • Not present in all message types
  • Native Timestamp
    • message.native_timestamp
    • Always represents when the sensor transmitted the data to the native interface
    • Native timestamps must be set as absolute UTC time, or relative UTC millisecond or microsecond time
    • message.native_timestamp.format
  • Scanning LiDAR contain two extra timestamp fields
  • Start-scan timestamp represents the start of the scan
  • End-scan timestamp represent the end of the scan

6.3.4 Transforms

Dynamic Driver interfaces use the Transform API to transform the data to the coordinate frame specified in the System Design File (SDF).

The default coordinate frame is the vehicle centered reference coordinate frame. This allows you to use PolySync Studio, or a similar visualization application, to view and reference all data from the same origin. This is especially powerful during application development, such as within a sensor fusion algorithm or an occupancy grid.

Transforms occur during the psync_interface_process_data function, and before the message is published to the PolySync bus.

6.3.5 Validating sensor data

This isn’t a function that is part of the state machine, but a validation function that should be created as part of the PAL initialization process.

The validation function should read data for a predetermined amount of time. If all data read in this interval is in the expected format and order, then the interface is considered to be validated.

If unknown bytes or CAN frames are seen, validation should fail.

6.4 Configure

The most basic configure function is going to create and set the reference to the internal data structure, interface_data_s, which all PALs define. Before the function returns, the interface data is invalidated.

6.5 Initialize

All resources for the Dynamic Driver interface should be allocated within this function. This includes:

  • Message type support
  • The asynchronous output queue, for input data
  • The reference to the replay data queue
  • The transform for this node
  • Set the message sensor descriptor, describing this hardware-based publisher node
  • Read and set the SDF Configuration
  • Verify I/O Parameters from the SDF
  • Handling command line arguments
  • Handling the runtime context, live vs replay

The initialize function is responsible for checking the validity of the reference to the interface_data_s that is passed around within the ps_dynamic_driver structure. Next, command line arguments are parsed and processed accordingly.

6.5.1 Checking for replay

While parsing the command line options, the interface should check whether this node was started under the live (hardware) context, or the replay (no-hardware) context. If the interface is starting under replay context , then the routines to open, configure, and connect are skipped, otherwise they would throw a DTC and exit.

If the node is operating under the replay context, then dynamic_driver -> standard_options[STDOPT_NOHW] will be non-zero.

6.6 Validation

It’s common to call the validation function after initialization, and after or while recovering from a fault. Typically the node will spin for a specified time to read data and ensure that the node is in a known-good state.

6.7 Poll for data

While in poll for data, the interface PAL should perform a check for the runtime context. If the node is operating in the the replay (no-hardware) context, then a dummy-read could be performed. Next, the Dynamic Driver checks for a replay message in the queue. When a message is seen in the queue, it’s popped off and the data is copied into the internal message reference.

If the node is operating under the replay-context, then dynamic_driver -> logfile_mode will be LOGFILE_MODE_READ.

If the Runtime context is set to the live context , then data is read directly from the hardware and is then copied into the internal message reference.

6.7.1 Ethernet UDP sensors

If every UDP packet contains all of the information needed to populate a PolySync message, it is ok to block in the read call, but it needs to contain a very small timeout mechanism. It is recommended to do non-blocking polling. If necessary, block for 10-20 milliseconds for minimum disruption.

6.8 Log data

While in the log data function, the interface PAL should perform a check for the runtime context. If the node is operating in the replay (no-hardware) context, then you can copy the internal message reference into the PolySync CAN or Byte Array message. Once the data is copied, the message can be written using the Logfile API.

6.8.1 Knowing what message type to log

The PAL should log the lowest-level of data possible, typically the raw data type that is returned by the Device Communication APIs. For CAN sensors, the ps_can_frame_msg is ideal. For most other sensors, the ps_byte_array_msg is capable of holding the binary data.

6.9 Process data

The function will only be called if the previous call to psync_interface_poll_for_data returned data available (i.e. DTC_NONE).

After checking the incoming data payload, the process data function starts dissecting the bytes. If the proper data exists, it can be abstracted into a PolySync message, then transformed to the proper coordinate frame, per the SDF configuration.

It’s common for process data to publish the message to the PolySync bus, directly after applying the message -> header.timestamp field.

  • Check the data payload
  • Abstract into a PolySync message type
  • Transform data, depending on configuration
  • Get Tx timestamp
  • Publish

Avoid blocking in the process data routine. It is possible set up a command/response infrastructure with a timeout that gets checked in psync_interface_process_data, but the psync_interface_process_data function will only be called if the previous call to poll data returns data available (i.e. DTC_NONE).

PolySync provided interfaces contain a parameter to enable and disable publishing for each output message type. The publish toggle can also be implemented on the “raw data,” the ps_can_frame_msg, and/or the ps_byte_array_msg.

6.10 Idle

This is the junk-drawer function, and it is optional to implement. Any task that doesn’t fall directly in line with the functions specified above can be called here.

Examples are: * Petting the watchdog * Checking internal message queue (ego-motion again) * Update and publish diagnostic message

6.11 Handling parameters

Each interface is capable of supporting an infinite number of parameters. At any point during runtime, another node could send a parameter ‘set’ message─for example Studio’s System Hierarchy plugin. You can pick and choose which parameters need to be handled (set).

A good rule of thumb is that if the interface needs to reinitialize, then the parameter should be read-only. If the interface can dynamically update this parameter value and continue normal operation, then it should be handled in this function.

All nodes should support ‘set’ commands for mount position and orientation parameters.

6.12 Handle state

The PAL should check the dynamic_driver -> current_state -> dtc and perform the appropriate action for the DTC present.

For example: If DTC_NONATIVEBUS is encountered, the PAL should close the connection, re-open it, configure, validate, etc.

The state machine will only enter handle_state when a state other than OK is entered. The application will stay inside of the handle_state until psync_node_recover_fault is called.

Conclusion

Congratulations! Now you have the tools to create your own Dynamic Driver interface.