From 5648f301be4f78d61c9be47030f60834be0b2636 Mon Sep 17 00:00:00 2001
From: flipflip <flipflip@fixposition.com>
Date: Mon, 13 Jan 2025 07:56:27 +0100
Subject: [PATCH] Use Fixposition SDK, add devcontainer config, general cleanup

---
 .devcontainer/.bash_history.d/.gitkeep        |    0
 .devcontainer/.bash_logout                    |    9 +
 .devcontainer/.bashrc                         |  173 ++
 .devcontainer/.dummy-dir/.gitkeep             |    0
 .devcontainer/.dummy-file                     |    0
 .devcontainer/.gitignore                      |    2 +
 .devcontainer/.vscode-server/.gitkeep         |    0
 .devcontainer/helper.sh                       |   35 +
 .devcontainer/humble/Dockerfile               |   12 +
 .devcontainer/humble/devcontainer.json        |   53 +
 .devcontainer/jazzy/Dockerfile                |   12 +
 .devcontainer/jazzy/devcontainer.json         |   52 +
 .devcontainer/noetic/Dockerfile               |   12 +
 .devcontainer/noetic/devcontainer.json        |   52 +
 .github/ci.sh                                 |  162 ++
 .github/workflows/build_test_ros.yml          |   43 -
 .github/workflows/build_test_ros2.yml         |   43 -
 .github/workflows/ci.yml                      |   64 +
 .gitignore                                    |   16 +-
 .gitmodules                                   |    4 +
 .pre-commit-config.yaml                       |   15 +
 LICENSE                                       |    2 +-
 README.md                                     |   18 +-
 create_ros_ws.sh                              |  213 ++
 fixposition-sdk                               |    1 +
 fixposition_driver.code-workspace             |  237 ++
 fixposition_driver_lib/CMakeLists.txt         |   57 +-
 fixposition_driver_lib/Doxyfile               | 2427 -----------------
 fixposition_driver_lib/README.md              |   36 +-
 .../fixposition_driver.hpp                    |  255 +-
 .../fixposition_driver_lib/gnss_tf.hpp        |  164 --
 .../include/fixposition_driver_lib/helper.hpp |  277 +-
 .../messages/base_converter.hpp               |  268 --
 .../messages/fpa_type.hpp                     |  644 -----
 .../messages/fpb_measurements.hpp             |   73 -
 .../messages/fpb_type.hpp                     |   92 -
 .../messages/msg_data.hpp                     |  190 --
 .../messages/nmea_type.hpp                    |  567 ----
 .../messages/nov_type.hpp                     |  711 -----
 .../include/fixposition_driver_lib/params.hpp |   74 +-
 .../include/fixposition_driver_lib/parser.hpp |   43 -
 .../time_conversions.hpp                      |  271 --
 fixposition_driver_lib/package.xml            |   13 +-
 .../src/fixposition_driver.cpp                |  882 +++---
 fixposition_driver_lib/src/gnss_tf.cpp        |  197 --
 fixposition_driver_lib/src/helper.cpp         |  359 ++-
 .../src/messages/fpa/corrimu.cpp              |   63 -
 .../src/messages/fpa/eoe.cpp                  |   57 -
 .../src/messages/fpa/gnssant.cpp              |   67 -
 .../src/messages/fpa/gnsscorr.cpp             |   85 -
 .../src/messages/fpa/imubias.cpp              |   80 -
 .../src/messages/fpa/llh.cpp                  |   70 -
 .../src/messages/fpa/odomenu.cpp              |  139 -
 .../src/messages/fpa/odometry.cpp             |  141 -
 .../src/messages/fpa/odomsh.cpp               |  139 -
 .../src/messages/fpa/odomstatus.cpp           |  111 -
 .../src/messages/fpa/rawimu.cpp               |   63 -
 .../src/messages/fpa/text.cpp                 |   54 -
 .../src/messages/fpa/tf.cpp                   |   74 -
 .../src/messages/fpa/tp.cpp                   |   62 -
 .../src/messages/nmea/gngsa.cpp               |   64 -
 .../src/messages/nmea/gpgga.cpp               |   96 -
 .../src/messages/nmea/gpgll.cpp               |   67 -
 .../src/messages/nmea/gpgst.cpp               |   55 -
 .../src/messages/nmea/gphdt.cpp               |   39 -
 .../src/messages/nmea/gprmc.cpp               |   81 -
 .../src/messages/nmea/gpvtg.cpp               |   57 -
 .../src/messages/nmea/gpzda.cpp               |   62 -
 .../src/messages/nmea/gxgsv.cpp               |   61 -
 .../src/messages/nmea/nmea.cpp                |  155 --
 fixposition_driver_lib/src/params.cpp         |   46 +
 fixposition_driver_lib/src/parser.cpp         |  139 -
 fixposition_driver_msgs/CMakeLists.txt        |  149 +
 fixposition_driver_msgs/README.md             |    3 +
 .../fixposition_driver_msgs/data_to_ros.hpp   |  661 +++++
 fixposition_driver_msgs/msg/CovWarn.msg       |    8 +
 fixposition_driver_msgs/msg/FpaConsts.msg     |  198 ++
 fixposition_driver_msgs/msg/FpaEoe.msg        |    9 +
 fixposition_driver_msgs/msg/FpaGnssant.msg    |   14 +
 fixposition_driver_msgs/msg/FpaGnsscorr.msg   |   23 +
 fixposition_driver_msgs/msg/FpaImubias.msg    |   16 +
 fixposition_driver_msgs/msg/FpaLlh.msg        |   10 +
 fixposition_driver_msgs/msg/FpaOdomenu.msg    |   19 +
 fixposition_driver_msgs/msg/FpaOdometry.msg   |   21 +
 fixposition_driver_msgs/msg/FpaOdomsh.msg     |   19 +
 fixposition_driver_msgs/msg/FpaOdomstatus.msg |   28 +
 fixposition_driver_msgs/msg/FpaText.msg       |   10 +
 fixposition_driver_msgs/msg/FpaTp.msg         |   14 +
 fixposition_driver_msgs/msg/NmeaConsts.msg    |  105 +
 fixposition_driver_msgs/msg/NmeaEpoch.msg     |   64 +
 fixposition_driver_msgs/msg/NmeaGga.msg       |   25 +
 fixposition_driver_msgs/msg/NmeaGll.msg       |   19 +
 fixposition_driver_msgs/msg/NmeaGsa.msg       |   18 +
 fixposition_driver_msgs/msg/NmeaGst.msg       |   21 +
 fixposition_driver_msgs/msg/NmeaGsv.msg       |   20 +
 fixposition_driver_msgs/msg/NmeaHdt.msg       |   10 +
 fixposition_driver_msgs/msg/NmeaRmc.msg       |   29 +
 fixposition_driver_msgs/msg/NmeaSatellite.msg |   11 +
 fixposition_driver_msgs/msg/NmeaSignal.msg    |   12 +
 fixposition_driver_msgs/msg/NmeaVtg.msg       |   15 +
 fixposition_driver_msgs/msg/NmeaZda.msg       |   21 +
 fixposition_driver_msgs/msg/ParserMsg.msg     |   23 +
 fixposition_driver_msgs/msg/Speed.msg         |    7 +
 fixposition_driver_msgs/msg/WheelSensor.msg   |   20 +
 fixposition_driver_msgs/package.xml           |   35 +
 fixposition_driver_msgs/src/data_to_ros.cpp   |   25 +
 fixposition_driver_ros1/CMakeLists.txt        |   70 +-
 fixposition_driver_ros1/Doxyfile              | 2427 -----------------
 fixposition_driver_ros1/README.md             |   18 +-
 .../fixposition_driver_ros1/data_to_ros1.hpp  |  201 +-
 .../fixposition_driver_node.hpp               |  196 +-
 .../fixposition_driver_ros1/params.hpp        |   59 +-
 .../fixposition_driver_ros1/ros1_msgs.hpp     |   57 +
 .../fixposition_driver_ros1/ros_msgs.hpp      |   66 -
 fixposition_driver_ros1/launch/config.yaml    |   58 +
 fixposition_driver_ros1/launch/dev.launch     |    9 +
 .../launch/driver_with_odom_converter.launch  |    4 +-
 fixposition_driver_ros1/launch/node.launch    |    9 +
 .../launch/rosconsole.conf                    |    4 +-
 .../launch/rosconsole_dev.conf                |    2 +
 fixposition_driver_ros1/launch/serial.launch  |   16 -
 fixposition_driver_ros1/launch/serial.yaml    |   17 -
 fixposition_driver_ros1/launch/tcp.launch     |   16 -
 fixposition_driver_ros1/launch/tcp.yaml       |   17 -
 fixposition_driver_ros1/msg/GnssSats.msg      |   23 -
 fixposition_driver_ros1/msg/NMEA.msg          |   65 -
 fixposition_driver_ros1/msg/Speed.msg         |   17 -
 fixposition_driver_ros1/msg/WheelSensor.msg   |   29 -
 .../msg/extras/CovWarn.msg                    |   18 -
 fixposition_driver_ros1/msg/fpa/eoe.msg       |   17 -
 fixposition_driver_ros1/msg/fpa/gnssant.msg   |   22 -
 fixposition_driver_ros1/msg/fpa/gnsscorr.msg  |   31 -
 fixposition_driver_ros1/msg/fpa/imubias.msg   |   71 -
 fixposition_driver_ros1/msg/fpa/llh.msg       |   19 -
 fixposition_driver_ros1/msg/fpa/odomenu.msg   |   27 -
 fixposition_driver_ros1/msg/fpa/odometry.msg  |   28 -
 fixposition_driver_ros1/msg/fpa/odomsh.msg    |   27 -
 .../msg/fpa/odomstatus.msg                    |  190 --
 fixposition_driver_ros1/msg/fpa/text.msg      |   18 -
 fixposition_driver_ros1/msg/fpa/tp.msg        |   56 -
 fixposition_driver_ros1/msg/nmea/gngsa.msg    |   24 -
 fixposition_driver_ros1/msg/nmea/gpgga.msg    |   40 -
 fixposition_driver_ros1/msg/nmea/gpgll.msg    |   35 -
 fixposition_driver_ros1/msg/nmea/gpgst.msg    |   25 -
 fixposition_driver_ros1/msg/nmea/gphdt.msg    |   19 -
 fixposition_driver_ros1/msg/nmea/gprmc.msg    |   38 -
 fixposition_driver_ros1/msg/nmea/gpvtg.msg    |   37 -
 fixposition_driver_ros1/msg/nmea/gpzda.msg    |   22 -
 fixposition_driver_ros1/msg/nmea/gxgsv.msg    |   39 -
 fixposition_driver_ros1/package.xml           |   12 +-
 fixposition_driver_ros1/src/data_to_ros1.cpp  | 1185 ++++----
 .../src/fixposition_driver_node.cpp           | 1011 ++++---
 fixposition_driver_ros1/src/params.cpp        |  172 +-
 fixposition_driver_ros2/CMakeLists.txt        |   58 +-
 fixposition_driver_ros2/Doxyfile              | 2427 -----------------
 fixposition_driver_ros2/README.md             |   18 +-
 .../fixposition_driver_ros2/data_to_ros2.hpp  |  153 +-
 .../fixposition_driver_node.hpp               |  223 +-
 .../fixposition_driver_ros2/params.hpp        |   65 +-
 .../fixposition_driver_ros2/ros2_msgs.hpp     |  104 +-
 fixposition_driver_ros2/launch/config.yaml    |   61 +
 fixposition_driver_ros2/launch/dev.launch     |   12 +
 fixposition_driver_ros2/launch/node.launch    |   12 +
 fixposition_driver_ros2/launch/serial.launch  |   11 -
 fixposition_driver_ros2/launch/serial.yaml    |   19 -
 fixposition_driver_ros2/launch/tcp.launch     |   12 -
 fixposition_driver_ros2/launch/tcp.yaml       |   20 -
 fixposition_driver_ros2/msg/Gnsssats.msg      |   20 -
 fixposition_driver_ros2/msg/NMEA.msg          |   63 -
 fixposition_driver_ros2/msg/Speed.msg         |   14 -
 fixposition_driver_ros2/msg/WheelSensor.msg   |   26 -
 .../msg/extras/COVWARN.msg                    |   15 -
 fixposition_driver_ros2/msg/fpa/EOE.msg       |   14 -
 fixposition_driver_ros2/msg/fpa/GNSSANT.msg   |   19 -
 fixposition_driver_ros2/msg/fpa/GNSSCORR.msg  |   28 -
 fixposition_driver_ros2/msg/fpa/IMUBIAS.msg   |   68 -
 fixposition_driver_ros2/msg/fpa/LLH.msg       |   16 -
 fixposition_driver_ros2/msg/fpa/ODOMENU.msg   |   24 -
 fixposition_driver_ros2/msg/fpa/ODOMETRY.msg  |   25 -
 fixposition_driver_ros2/msg/fpa/ODOMSH.msg    |   24 -
 .../msg/fpa/ODOMSTATUS.msg                    |  187 --
 fixposition_driver_ros2/msg/fpa/TEXT.msg      |   15 -
 fixposition_driver_ros2/msg/fpa/TP.msg        |   53 -
 fixposition_driver_ros2/msg/nmea/GNGSA.msg    |   21 -
 fixposition_driver_ros2/msg/nmea/GPGGA.msg    |   37 -
 fixposition_driver_ros2/msg/nmea/GPGLL.msg    |   32 -
 fixposition_driver_ros2/msg/nmea/GPGST.msg    |   22 -
 fixposition_driver_ros2/msg/nmea/GPHDT.msg    |   16 -
 fixposition_driver_ros2/msg/nmea/GPRMC.msg    |   35 -
 fixposition_driver_ros2/msg/nmea/GPVTG.msg    |   34 -
 fixposition_driver_ros2/msg/nmea/GPZDA.msg    |   19 -
 fixposition_driver_ros2/msg/nmea/GXGSV.msg    |   36 -
 fixposition_driver_ros2/package.xml           |   11 +-
 fixposition_driver_ros2/src/data_to_ros2.cpp  | 1206 ++++----
 .../src/fixposition_driver_node.cpp           | 1073 +++++---
 fixposition_driver_ros2/src/params.cpp        |  197 +-
 .../CMakeLists.txt                            |    2 +-
 fixposition_odometry_converter_ros1/README.md |   21 +-
 .../odom_converter.hpp                        |   32 +-
 .../params.hpp                                |   30 +-
 .../ros_msgs.hpp                              |   21 +-
 .../package.xml                               |   10 +-
 .../src/odom_converter.cpp                    |   30 +-
 .../src/odom_converter_node.cpp               |   21 +-
 .../src/params.cpp                            |   26 +-
 .../CMakeLists.txt                            |    4 +-
 fixposition_odometry_converter_ros2/README.md |   21 +-
 .../odom_converter_node.hpp                   |   34 +-
 .../params.hpp                                |   32 +-
 .../ros2_msgs.hpp                             |   15 +-
 .../package.xml                               |   30 +-
 .../src/odom_converter_node.cpp               |   33 +-
 .../src/params.cpp                            |   25 +-
 precommit.sh                                  |    8 +
 214 files changed, 7646 insertions(+), 18496 deletions(-)
 create mode 100644 .devcontainer/.bash_history.d/.gitkeep
 create mode 100644 .devcontainer/.bash_logout
 create mode 100644 .devcontainer/.bashrc
 create mode 100644 .devcontainer/.dummy-dir/.gitkeep
 create mode 100644 .devcontainer/.dummy-file
 create mode 100644 .devcontainer/.gitignore
 create mode 100644 .devcontainer/.vscode-server/.gitkeep
 create mode 100755 .devcontainer/helper.sh
 create mode 100644 .devcontainer/humble/Dockerfile
 create mode 100644 .devcontainer/humble/devcontainer.json
 create mode 100644 .devcontainer/jazzy/Dockerfile
 create mode 100644 .devcontainer/jazzy/devcontainer.json
 create mode 100644 .devcontainer/noetic/Dockerfile
 create mode 100644 .devcontainer/noetic/devcontainer.json
 create mode 100755 .github/ci.sh
 delete mode 100644 .github/workflows/build_test_ros.yml
 delete mode 100644 .github/workflows/build_test_ros2.yml
 create mode 100644 .github/workflows/ci.yml
 create mode 100644 .gitmodules
 create mode 100644 .pre-commit-config.yaml
 create mode 100755 create_ros_ws.sh
 create mode 160000 fixposition-sdk
 create mode 100644 fixposition_driver.code-workspace
 delete mode 100644 fixposition_driver_lib/Doxyfile
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp
 delete mode 100644 fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp
 delete mode 100644 fixposition_driver_lib/src/gnss_tf.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/corrimu.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/eoe.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/gnssant.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/imubias.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/llh.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/odomenu.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/odometry.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/odomsh.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/odomstatus.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/rawimu.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/text.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/tf.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/fpa/tp.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gngsa.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gpgga.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gpgll.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gpgst.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gphdt.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gprmc.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gpvtg.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gpzda.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/gxgsv.cpp
 delete mode 100644 fixposition_driver_lib/src/messages/nmea/nmea.cpp
 create mode 100644 fixposition_driver_lib/src/params.cpp
 delete mode 100644 fixposition_driver_lib/src/parser.cpp
 create mode 100644 fixposition_driver_msgs/CMakeLists.txt
 create mode 100644 fixposition_driver_msgs/README.md
 create mode 100644 fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp
 create mode 100644 fixposition_driver_msgs/msg/CovWarn.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaConsts.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaEoe.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaGnssant.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaGnsscorr.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaImubias.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaLlh.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaOdomenu.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaOdometry.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaOdomsh.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaOdomstatus.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaText.msg
 create mode 100644 fixposition_driver_msgs/msg/FpaTp.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaConsts.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaEpoch.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaGga.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaGll.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaGsa.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaGst.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaGsv.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaHdt.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaRmc.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaSatellite.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaSignal.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaVtg.msg
 create mode 100644 fixposition_driver_msgs/msg/NmeaZda.msg
 create mode 100644 fixposition_driver_msgs/msg/ParserMsg.msg
 create mode 100644 fixposition_driver_msgs/msg/Speed.msg
 create mode 100644 fixposition_driver_msgs/msg/WheelSensor.msg
 create mode 100644 fixposition_driver_msgs/package.xml
 create mode 100644 fixposition_driver_msgs/src/data_to_ros.cpp
 delete mode 100644 fixposition_driver_ros1/Doxyfile
 create mode 100644 fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp
 delete mode 100644 fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp
 create mode 100644 fixposition_driver_ros1/launch/config.yaml
 create mode 100644 fixposition_driver_ros1/launch/dev.launch
 create mode 100644 fixposition_driver_ros1/launch/node.launch
 create mode 100644 fixposition_driver_ros1/launch/rosconsole_dev.conf
 delete mode 100644 fixposition_driver_ros1/launch/serial.launch
 delete mode 100644 fixposition_driver_ros1/launch/serial.yaml
 delete mode 100644 fixposition_driver_ros1/launch/tcp.launch
 delete mode 100644 fixposition_driver_ros1/launch/tcp.yaml
 delete mode 100644 fixposition_driver_ros1/msg/GnssSats.msg
 delete mode 100644 fixposition_driver_ros1/msg/NMEA.msg
 delete mode 100644 fixposition_driver_ros1/msg/Speed.msg
 delete mode 100644 fixposition_driver_ros1/msg/WheelSensor.msg
 delete mode 100644 fixposition_driver_ros1/msg/extras/CovWarn.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/eoe.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/gnssant.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/gnsscorr.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/imubias.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/llh.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/odomenu.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/odometry.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/odomsh.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/odomstatus.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/text.msg
 delete mode 100644 fixposition_driver_ros1/msg/fpa/tp.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gngsa.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gpgga.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gpgll.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gpgst.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gphdt.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gprmc.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gpvtg.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gpzda.msg
 delete mode 100644 fixposition_driver_ros1/msg/nmea/gxgsv.msg
 delete mode 100644 fixposition_driver_ros2/Doxyfile
 create mode 100644 fixposition_driver_ros2/launch/config.yaml
 create mode 100644 fixposition_driver_ros2/launch/dev.launch
 create mode 100644 fixposition_driver_ros2/launch/node.launch
 delete mode 100644 fixposition_driver_ros2/launch/serial.launch
 delete mode 100644 fixposition_driver_ros2/launch/serial.yaml
 delete mode 100644 fixposition_driver_ros2/launch/tcp.launch
 delete mode 100644 fixposition_driver_ros2/launch/tcp.yaml
 delete mode 100644 fixposition_driver_ros2/msg/Gnsssats.msg
 delete mode 100644 fixposition_driver_ros2/msg/NMEA.msg
 delete mode 100644 fixposition_driver_ros2/msg/Speed.msg
 delete mode 100644 fixposition_driver_ros2/msg/WheelSensor.msg
 delete mode 100644 fixposition_driver_ros2/msg/extras/COVWARN.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/EOE.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/GNSSANT.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/GNSSCORR.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/IMUBIAS.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/LLH.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/ODOMENU.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/ODOMETRY.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/ODOMSH.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/ODOMSTATUS.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/TEXT.msg
 delete mode 100644 fixposition_driver_ros2/msg/fpa/TP.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GNGSA.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GPGGA.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GPGLL.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GPGST.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GPHDT.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GPRMC.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GPVTG.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GPZDA.msg
 delete mode 100644 fixposition_driver_ros2/msg/nmea/GXGSV.msg
 create mode 100755 precommit.sh

diff --git a/.devcontainer/.bash_history.d/.gitkeep b/.devcontainer/.bash_history.d/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/.devcontainer/.bash_logout b/.devcontainer/.bash_logout
new file mode 100644
index 0000000..9265fd4
--- /dev/null
+++ b/.devcontainer/.bash_logout
@@ -0,0 +1,9 @@
+# ~/.bash_logout: executed by bash(1) when login shell exits.
+
+# save history
+history -a
+
+# when leaving the console clear the screen to increase privacy
+if [ "$SHLVL" = 1 ]; then
+    [ -x /usr/bin/clear_console ] && /usr/bin/clear_console -q
+fi
diff --git a/.devcontainer/.bashrc b/.devcontainer/.bashrc
new file mode 100644
index 0000000..e602d26
--- /dev/null
+++ b/.devcontainer/.bashrc
@@ -0,0 +1,173 @@
+# ~/.bashrc: executed by bash(1) for non-login shells.
+# see /usr/share/doc/bash/examples/startup-files (in the package bash-doc)
+# for examples
+
+# If not running interactively, don't do anything
+case $- in
+    *i*) ;;
+      *) return;;
+esac
+
+# don't put duplicate lines or lines starting with space in the history.
+# See bash(1) for more options
+HISTCONTROL="ignoredups:erasedups:ignorespace"
+
+# append to the history file, don't overwrite it
+shopt -s histappend
+
+# for setting history length see HISTSIZE and HISTFILESIZE in bash(1)
+HISTSIZE=10000
+HISTFILESIZE=10000000
+HISTFILE=/home/fpsdk/.bash_history.d/history
+
+# Disable history ! expansion
+set +H
+
+export RSYNC_RSH=ssh
+export PAGER="less -R -i -x 4 -+C"
+export PERLDOC_PAGER="less -R -i -x 4 -+C"
+export LESSCHARSET=UTF-8
+export EDITOR='vim'
+export GIT_EDITOR=vim
+
+# nicely coloured man pages
+export LESS_TERMCAP_mb=$'\E[01;31m'
+export LESS_TERMCAP_md=$'\E[01;31m'
+export LESS_TERMCAP_me=$'\E[0m'
+export LESS_TERMCAP_se=$'\E[0m'
+export LESS_TERMCAP_so=$'\E[01;44;33m'
+export LESS_TERMCAP_ue=$'\E[0m'
+export LESS_TERMCAP_us=$'\E[01;32m'
+
+export LESS="FRSX"
+
+# N.B. -R must be first or lesspipe.sh will be confused
+# warning: -R not compatible with +F (follow mode)!
+alias less='less -R -i -x 4 -S'
+
+# Enable core dumps (does it?)
+ulimit -c 1048576
+
+# check the window size after each command and, if necessary,
+# update the values of LINES and COLUMNS.
+shopt -s checkwinsize
+
+# If set, the pattern "**" used in a pathname expansion context will
+# match all files and zero or more directories and subdirectories.
+#shopt -s globstar
+
+# make less more friendly for non-text input files, see lesspipe(1)
+[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)"
+
+# set variable identifying the chroot you work in (used in the prompt below)
+if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then
+    debian_chroot=$(cat /etc/debian_chroot)
+fi
+
+# set a fancy prompt (non-color, unless we know we "want" color)
+case "$TERM" in
+    xterm-color|*-256color) color_prompt=yes;;
+esac
+
+# uncomment for a colored prompt, if the terminal has the capability; turned
+# off by default to not distract the user: the focus in a terminal window
+# should be on the output of commands, not on the prompt
+#force_color_prompt=yes
+
+if [ -n "$force_color_prompt" ]; then
+    if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then
+	# We have color support; assume it's compliant with Ecma-48
+	# (ISO/IEC-6429). (Lack of such support is extremely rare, and such
+	# a case would tend to support setf rather than setaf.)
+	color_prompt=yes
+    else
+	color_prompt=
+    fi
+fi
+
+if [ "$color_prompt" = yes ]; then
+    PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ '
+else
+    PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ '
+fi
+unset color_prompt force_color_prompt
+
+# If this is an xterm set the title to user@host:dir
+case "$TERM" in
+xterm*|rxvt*)
+    PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1"
+    ;;
+*)
+    ;;
+esac
+
+# enable color support of ls and also add handy aliases
+if [ -x /usr/bin/dircolors ]; then
+    test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)"
+    alias ls='ls --color=auto'
+    #alias dir='dir --color=auto'
+    #alias vdir='vdir --color=auto'
+
+    alias grep='grep --color=auto'
+    alias fgrep='fgrep --color=auto'
+    alias egrep='egrep --color=auto'
+fi
+
+# colored GCC warnings and errors
+#export GCC_COLORS='error=01;31:warning=01;35:note=01;36:caret=01;32:locus=01:quote=01'
+
+# some more ls aliases
+alias ll='ls -alF'
+alias la='ls -A'
+alias l='ls -CF'
+alias rm='rm -i'
+alias mv='mv -i'
+
+# Add an "alert" alias for long running commands.  Use like so:
+#   sleep 10; alert
+alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"'
+
+# Alias definitions.
+# You may want to put all your additions into a separate file like
+# ~/.bash_aliases, instead of adding them here directly.
+# See /usr/share/doc/bash-doc/examples in the bash-doc package.
+
+if [ -f ~/.bash_aliases ]; then
+    . ~/.bash_aliases
+fi
+
+# enable programmable completion features (you don't need to enable
+# this, if it's already enabled in /etc/bash.bashrc and /etc/profile
+# sources /etc/bash.bashrc).
+if ! shopt -oq posix; then
+  if [ -f /usr/share/bash-completion/bash_completion ]; then
+    . /usr/share/bash-completion/bash_completion
+  elif [ -f /etc/bash_completion ]; then
+    . /etc/bash_completion
+  fi
+fi
+
+if [ -f ~/.bash-git-prompt/gitprompt.sh ]; then
+    # https://github.com/magicmonty/bash-git-prompt
+    GIT_PROMPT_THEME=fpsdk
+    source ~/.bash-git-prompt/gitprompt.sh
+fi
+
+# Source ROS environment
+if [ -n "${ROS_DISTRO}" ]; then
+    source /opt/ros/${ROS_DISTRO}/setup.bash
+    export ROSCONSOLE_STDOUT_LINE_BUFFERED=1
+    export ROSCONSOLE_FORMAT='${severity} ${time:%Y-%m-%d %H:%M:%S.%f} ${logger} - ${message}'
+    # export RCUTILS_LOGGING_BUFFERED_STREAM=1
+    # export RCUTILS_LOGGING_USE_STDOUT=1
+    export RCUTILS_COLORIZED_OUTPUT=1
+    case "${ROS_DISTRO}" in
+        jazzy|rolling)
+            export RCUTILS_CONSOLE_OUTPUT_FORMAT="{severity} {date_time_with_ms} {name} - {message}"
+            ;;
+        *)
+            export RCUTILS_CONSOLE_OUTPUT_FORMAT="{severity} {time} {name} - {message}"
+            ;;
+    esac
+
+fi
diff --git a/.devcontainer/.dummy-dir/.gitkeep b/.devcontainer/.dummy-dir/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/.devcontainer/.dummy-file b/.devcontainer/.dummy-file
new file mode 100644
index 0000000..e69de29
diff --git a/.devcontainer/.gitignore b/.devcontainer/.gitignore
new file mode 100644
index 0000000..45409b0
--- /dev/null
+++ b/.devcontainer/.gitignore
@@ -0,0 +1,2 @@
+/.bash_history.d
+/.vscode-server
diff --git a/.devcontainer/.vscode-server/.gitkeep b/.devcontainer/.vscode-server/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/.devcontainer/helper.sh b/.devcontainer/helper.sh
new file mode 100755
index 0000000..3719d3b
--- /dev/null
+++ b/.devcontainer/helper.sh
@@ -0,0 +1,35 @@
+#!/bin/bash
+set -eEu -o pipefail
+
+command=$1
+shift
+
+case ${command} in
+    onCreateCommand)
+        containerWorkspaceFolder=$1
+        # ls -la ${containerWorkspaceFolder}
+        # id
+        case ${ROS_DISTRO} in
+            noetic)
+                if [ ! -d ${containerWorkspaceFolder}/ros1_ws ]; then
+                    ${containerWorkspaceFolder}/create_ros_ws.sh -d ros1_ws
+                fi
+                ;;
+            humble|jazzy)
+                if [ ! -d ${containerWorkspaceFolder}/ros2_ws ]; then
+                    ${containerWorkspaceFolder}/create_ros_ws.sh -d ros2_ws
+                fi
+                ;;
+            *)
+                echo "Don't know how to create a ROS workspace for ROS_DISTRO=${ROS_DISTRO}"
+                exit 1
+                ;;
+        esac
+        ;;
+    *)
+        echo "bad command: ${command}"
+        exit 1
+        ;;
+esac
+
+exit 0
diff --git a/.devcontainer/humble/Dockerfile b/.devcontainer/humble/Dockerfile
new file mode 100644
index 0000000..feb4f37
--- /dev/null
+++ b/.devcontainer/humble/Dockerfile
@@ -0,0 +1,12 @@
+FROM ghcr.io/fixposition/fixposition-sdk:humble-dev
+
+# RUN <<EOF
+#     # Install additional packages
+#     set -e
+#     export DEBIAN_FRONTEND=noninteractive
+#     apt-get -y update
+#     apt-get -y --with-new-pkgs upgrade
+#     apt-get -y --no-install-recommends install some-package another-package ...
+#     apt-get -y autoremove
+#     apt-get clean
+# EOF
diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json
new file mode 100644
index 0000000..de879c3
--- /dev/null
+++ b/.devcontainer/humble/devcontainer.json
@@ -0,0 +1,53 @@
+// https://aka.ms/devcontainer.json
+{
+    "name": "Fixposition ROS2 driver (Humble)",
+    "build": { "dockerfile": "Dockerfile", "context": "../.." },
+    "remoteUser": "fpsdk",
+    "updateRemoteUserUID": true,
+    "capAdd": ["SYS_PTRACE"],
+    "mounts": [
+        // Persistent bash history
+        "source=${localWorkspaceFolder}/.devcontainer/.bashrc,target=/home/fpsdk/.bashrc,type=bind",
+        "source=${localWorkspaceFolder}/.devcontainer/.bash_logout,target=/home/fpsdk/.bash_logout,type=bind",
+        "source=${localWorkspaceFolder}/.devcontainer/.bash_history.d,target=/home/fpsdk/.bash_history.d,type=bind",
+        // Persistent .vscode-server
+        "source=${localWorkspaceFolder}/.devcontainer/.vscode-server,target=/home/fpsdk/.vscode-server,type=bind",
+        // The code we're working on
+        "source=${localWorkspaceFolder},target=/home/fpsdk/fixposition_driver,type=bind",
+    ],
+    "workspaceFolder": "/home/fpsdk/fixposition_driver",
+    "runArgs": [ "--name=fixposition_driver-humble-${localEnv:USER}", "--hostname=humble" ],
+    "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/helper.sh onCreateCommand ${containerWorkspaceFolder}",
+    "customizations": {
+        "vscode": {
+            "settings": {
+                "files.exclude": {
+                    "**/.git": true,
+                    ".vscode-cache": true,
+                    "**/*~": true,
+                    ".vstags": true,
+                    "**/build/**": true,
+                    "core": true,
+                    "**/.devcontainer/.vscode-server/**": true,
+                    ".devcontainer/.vscode-server/**": true,
+                    "**/.vscode-server/**": true
+                },
+                "search.exclude": {
+                    "tmp": true,
+                    "**/ros1_ws/**": true,
+                    "**/ros2_ws/**": true
+                },
+                "files.watcherExclude": {
+                    "**/.git/objects/**": true,
+                    "**/.git/refs/**": true,
+                    "**/.git/logs/**": true,
+                    "**/.git/subtree-cache/**": true,
+                    "**/.git/worktrees/**": true,
+                    "**/.vscode-*/**": true,
+                    "**/ros1_ws/**": true,
+                    "**/ros2_ws/**": true
+                }
+            }
+        }
+    }
+}
diff --git a/.devcontainer/jazzy/Dockerfile b/.devcontainer/jazzy/Dockerfile
new file mode 100644
index 0000000..316ff54
--- /dev/null
+++ b/.devcontainer/jazzy/Dockerfile
@@ -0,0 +1,12 @@
+FROM ghcr.io/fixposition/fixposition-sdk:jazzy-dev
+
+# RUN <<EOF
+#     # Install additional packages
+#     set -e
+#     export DEBIAN_FRONTEND=noninteractive
+#     apt-get -y update
+#     apt-get -y --with-new-pkgs upgrade
+#     apt-get -y --no-install-recommends install some-package another-package ...
+#     apt-get -y autoremove
+#     apt-get clean
+# EOF
diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json
new file mode 100644
index 0000000..243aecd
--- /dev/null
+++ b/.devcontainer/jazzy/devcontainer.json
@@ -0,0 +1,52 @@
+// https://aka.ms/devcontainer.json
+{
+    "name": "Fixposition ROS2 driver (Jazzy)",
+    "build": { "dockerfile": "Dockerfile", "context": "../.." },
+    "remoteUser": "fpsdk",
+    "updateRemoteUserUID": true,
+    "capAdd": ["SYS_PTRACE"],
+    "mounts": [
+        // Persistent bash history
+        "source=${localWorkspaceFolder}/.devcontainer/.bashrc,target=/home/fpsdk/.bashrc,type=bind",
+        "source=${localWorkspaceFolder}/.devcontainer/.bash_logout,target=/home/fpsdk/.bash_logout,type=bind",
+        "source=${localWorkspaceFolder}/.devcontainer/.bash_history.d,target=/home/fpsdk/.bash_history.d,type=bind",
+        // Persistent .vscode-server
+        "source=${localWorkspaceFolder}/.devcontainer/.vscode-server,target=/home/fpsdk/.vscode-server,type=bind",
+        // The code we're working on
+        "source=${localWorkspaceFolder},target=/home/fpsdk/fixposition_driver,type=bind",
+    ],
+    "workspaceFolder": "/home/fpsdk/fixposition_driver",
+    "runArgs": [ "--name=fixposition_driver-jazzy-${localEnv:USER}", "--hostname=jazzy" ],
+    "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/helper.sh onCreateCommand ${containerWorkspaceFolder}",
+    "customizations": {
+        "vscode": {
+            "settings": {
+                "files.exclude": {
+                    "**/.git": true,
+                    ".vscode-cache": true,
+                    "**/*~": true,
+                    ".vstags": true,
+                    "**/build/**": true,
+                    "core": true,
+                    "**/.devcontainer/.vscode-server/**": true,
+                    "**/.vscode-server/**": true
+                },
+                "search.exclude": {
+                    "tmp": true,
+                    "**/ros1_ws/**": true,
+                    "**/ros2_ws/**": true
+                },
+                "files.watcherExclude": {
+                    "**/.git/objects/**": true,
+                    "**/.git/refs/**": true,
+                    "**/.git/logs/**": true,
+                    "**/.git/subtree-cache/**": true,
+                    "**/.git/worktrees/**": true,
+                    "**/.vscode-*/**": true,
+                    "**/ros1_ws/**": true,
+                    "**/ros2_ws/**": true
+                }
+            }
+        }
+    }
+}
diff --git a/.devcontainer/noetic/Dockerfile b/.devcontainer/noetic/Dockerfile
new file mode 100644
index 0000000..c60690b
--- /dev/null
+++ b/.devcontainer/noetic/Dockerfile
@@ -0,0 +1,12 @@
+FROM ghcr.io/fixposition/fixposition-sdk:noetic-dev
+
+# RUN <<EOF
+#     # Install additional packages
+#     set -e
+#     export DEBIAN_FRONTEND=noninteractive
+#     apt-get -y update
+#     apt-get -y --with-new-pkgs upgrade
+#     apt-get -y --no-install-recommends install some-package another-package ...
+#     apt-get -y autoremove
+#     apt-get clean
+# EOF
diff --git a/.devcontainer/noetic/devcontainer.json b/.devcontainer/noetic/devcontainer.json
new file mode 100644
index 0000000..6315d3e
--- /dev/null
+++ b/.devcontainer/noetic/devcontainer.json
@@ -0,0 +1,52 @@
+// https://aka.ms/devcontainer.json
+{
+    "name": "Fixposition ROS1 driver (Noetic)",
+    "build": { "dockerfile": "Dockerfile", "context": "../.." },
+    "remoteUser": "fpsdk",
+    "updateRemoteUserUID": true,
+    "capAdd": ["SYS_PTRACE"],
+    "mounts": [
+        // Persistent bash history
+        "source=${localWorkspaceFolder}/.devcontainer/.bashrc,target=/home/fpsdk/.bashrc,type=bind",
+        "source=${localWorkspaceFolder}/.devcontainer/.bash_logout,target=/home/fpsdk/.bash_logout,type=bind",
+        "source=${localWorkspaceFolder}/.devcontainer/.bash_history.d,target=/home/fpsdk/.bash_history.d,type=bind",
+        // Persistent .vscode-server
+        "source=${localWorkspaceFolder}/.devcontainer/.vscode-server,target=/home/fpsdk/.vscode-server,type=bind",
+        // The code we're working on
+        "source=${localWorkspaceFolder},target=/home/fpsdk/fixposition_driver,type=bind",
+    ],
+    "workspaceFolder": "/home/fpsdk/fixposition_driver",
+    "runArgs": [ "--name=fixposition_driver-noetic-${localEnv:USER}", "--hostname=noetic" ],
+    "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/helper.sh onCreateCommand ${containerWorkspaceFolder}",
+    "customizations": {
+        "vscode": {
+            "settings": {
+                "files.exclude": {
+                    "**/.git": true,
+                    ".vscode-cache": true,
+                    "**/*~": true,
+                    "**/build/**": true,
+                    ".vstags": true,
+                    "core": true,
+                    "**/.devcontainer/.vscode-server/**": true,
+                    "**/.vscode-server/**": true
+                },
+                "search.exclude": {
+                    "tmp": true,
+                    "**/ros1_ws/**": true,
+                    "**/ros2_ws/**": true
+                },
+                "files.watcherExclude": {
+                    "**/.git/objects/**": true,
+                    "**/.git/refs/**": true,
+                    "**/.git/logs/**": true,
+                    "**/.git/subtree-cache/**": true,
+                    "**/.git/worktrees/**": true,
+                    "**/.vscode-*/**": true,
+                    "**/ros1_ws/**": true,
+                    "**/ros2_ws/**": true
+                }
+            }
+        }
+    }
+}
diff --git a/.github/ci.sh b/.github/ci.sh
new file mode 100755
index 0000000..7d57d38
--- /dev/null
+++ b/.github/ci.sh
@@ -0,0 +1,162 @@
+#!/bin/bash
+set -eEu
+set -o pipefail
+set -o errtrace
+
+SCRIPTDIR=$(dirname $(readlink -f $0))
+
+# ----------------------------------------------------------------------------------------------------------------------
+
+# Variables set in our Dockerfiles. Try to make it work in other environments.
+if [ -z "${FPSDK_IMAGE:-}" ]; then
+    export FPSDK_IMAGE=other
+    if [ -z "${ROS_DISTRO:-}" ]; then
+        if [ -d /opt/ros/noetic ]; then
+            export ROS_DISTRO=noetic
+        elif [ -d /opt/ros/humble ]; then
+            export ROS_DISTRO=humble
+        elif [ -d /opt/ros/jazzy ]; then
+            export ROS_DISTRO=jazzy
+        else
+            export ROS_DISTRO=
+        fi
+    fi
+
+    echo "Not using a Fixposition SDK Docker image (ROS_DISTRO=${ROS_DISTRO})"
+
+else
+    echo "Using Fixposition SDK Docker image (FPSDK_IMAGE=${FPSDK_IMAGE}, ROS_DISTRO=${ROS_DISTRO})"
+fi
+
+# Sources will be here
+if [ -n "${GITHUB_WORKSPACE:-}" ]; then
+    FP_SRC_DIR=${GITHUB_WORKSPACE}
+# For running it locally via docker.sh...
+else
+    FP_SRC_DIR=${SCRIPTDIR}/..
+fi
+echo "FP_SRC_DIR=${FP_SRC_DIR}"
+
+# Optional single command-line argument to ci.sh to select only one (or some) step(s)
+STEP_FILT=${1:-}
+
+# ----------------------------------------------------------------------------------------------------------------------
+
+ERROR_COUNT=0
+ERROR_NAMES=
+NSTEPS=0
+declare -A TITLES
+function do_step
+{
+    local func=$1
+
+    # Skip?
+    if [[ ! ${func} =~ ${STEP_FILT} ]]; then
+        echo "Skip ${TITLES[$func]}"
+        return 0
+    fi
+
+    local res=0
+    echo "::group::${TITLES[$func]} ($func)"
+    echo "----- $func: ${TITLES[$func]} -----"
+    ((NSTEPS=${NSTEPS} + 1))
+
+    if ! ${func}; then
+        res=1
+        ((ERROR_COUNT=${ERROR_COUNT} + 1))
+        ERROR_NAMES="${ERROR_NAMES} ${func}"
+    fi
+
+    echo "::endgroup::"
+    if [ ${res} -ne 0 ]; then
+        echo "::warning title=${FPSDK_IMAGE} ${func} failed::${TITLES[$func]} ($func)"
+    fi
+
+    return ${res}
+}
+
+
+########################################################################################################################
+
+TITLES["pre_commit_check"]="Pre-commit checks"
+function pre_commit_check
+{
+    ${FP_SRC_DIR}/precommit.sh
+}
+
+########################################################################################################################
+
+TITLES["build_catkin_release"]="Build catkin (release, with ROS1)"
+function build_catkin_release
+{
+    local buildname=${FPSDK_IMAGE}_build_catkin_release
+    ${FP_SRC_DIR}/create_ros_ws.sh ${buildname}
+    cd ${FP_SRC_DIR}/${buildname}
+    catkin build || return 1
+}
+
+TITLES["build_catkin_debug"]="Build catkin (debug, with ROS1)"
+function build_catkin_debug
+{
+    local buildname=${FPSDK_IMAGE}_build_catkin_debug
+    ${FP_SRC_DIR}/create_ros_ws.sh -d ${buildname}
+    cd ${FP_SRC_DIR}/${buildname}
+    catkin build || return 1
+}
+
+########################################################################################################################
+
+TITLES["build_colcon_release"]="Build colcon (release, with ROS2)"
+function build_colcon_release
+{
+    local buildname=${FPSDK_IMAGE}_build_colcon_release
+    ${FP_SRC_DIR}/create_ros_ws.sh ${buildname} || return 1
+    cd ${FP_SRC_DIR}/${buildname}
+    colcon build || return 1
+}
+
+TITLES["build_colcon_debug"]="Build colcon (debug, with ROS2)"
+function build_colcon_debug
+{
+    local buildname=${FPSDK_IMAGE}_build_colcon_debug
+    ${FP_SRC_DIR}/create_ros_ws.sh -d ${buildname} || return 1
+    cd ${FP_SRC_DIR}/${buildname}
+    colcon build || return 1
+}
+
+########################################################################################################################
+
+# Always
+do_step pre_commit_check               || true # continue
+
+# ROS 1
+if [ "${ROS_DISTRO}" = "noetic" ]; then
+    set +u
+    source /opt/ros/${ROS_DISTRO}/setup.bash
+    set -u
+
+    do_step build_catkin_release          || true # continue
+    do_step build_catkin_debug            || true # continue
+
+# ROS 2
+elif [ "${ROS_DISTRO}" = "humble" -o "${ROS_DISTRO}" = "jazzy" ]; then
+    set +u
+    source /opt/ros/${ROS_DISTRO}/setup.bash
+    set -u
+
+    do_step build_colcon_release          || true # continue
+    do_step build_colcon_debug            || true # continue
+fi
+
+########################################################################################################################
+
+# Are we happy?
+if [ ${ERROR_COUNT} -eq 0 ]; then
+    echo "::notice TITLES=CI success::Successfully completed ${NSTEPS} steps"
+    exit 0
+else
+    echo "::error TITLES=CI failure::Failed ${ERROR_COUNT} of ${NSTEPS} steps: ${ERROR_NAMES}"
+    exit 1
+fi
+
+########################################################################################################################
diff --git a/.github/workflows/build_test_ros.yml b/.github/workflows/build_test_ros.yml
deleted file mode 100644
index 1445c7d..0000000
--- a/.github/workflows/build_test_ros.yml
+++ /dev/null
@@ -1,43 +0,0 @@
-name: Build and Test with ROS1
-
-on:
-  push:
-    branches: [main]
-  pull_request:
-    branches: [main]
-
-jobs:
-  build:
-    name: ${{ matrix.config.name }}
-    runs-on: ubuntu-latest
-
-    strategy:
-      fail-fast: false
-      matrix:
-        config:
-          - name: "noetic"
-            container:
-              image: "ghcr.io/fixposition/fixposition-sdk:noetic-ci"
-
-    container: ${{ matrix.config.container }}
-
-    defaults:
-      run:
-        shell: bash
-
-    steps:
-      - uses: actions/checkout@v4
-        with:
-          path: src/fixposition_driver
-      - name: Ignore ROS2 node
-        run: |
-          touch src/fixposition_driver/fixposition_driver_ros2/CATKIN_IGNORE
-          touch src/fixposition_driver/fixposition_odometry_converter_ros2/CATKIN_IGNORE
-      - name: Init Workspace
-        run: |
-          catkin init
-      - name: Build and Test
-        run: |
-          source /opt/ros/$ROS_DISTRO/setup.bash
-          catkin build fixposition_driver_lib fixposition_driver_ros1 fixposition_odometry_converter_ros1 --force-cmake -DBUILD_TESTING=ON
-          catkin run_tests
diff --git a/.github/workflows/build_test_ros2.yml b/.github/workflows/build_test_ros2.yml
deleted file mode 100644
index 9d810be..0000000
--- a/.github/workflows/build_test_ros2.yml
+++ /dev/null
@@ -1,43 +0,0 @@
-name: Build and Test with ROS2
-
-on:
-  push:
-    branches: [main]
-  pull_request:
-    branches: [main]
-
-jobs:
-  build:
-    name: ${{ matrix.config.name }}
-    runs-on: ubuntu-latest
-
-    strategy:
-      fail-fast: false
-      matrix:
-        config:
-          - name: "humble"
-            container:
-              image: "ghcr.io/fixposition/fixposition-sdk:humble-ci"
-          - name: "jazzy"
-            container:
-              image: "ghcr.io/fixposition/fixposition-sdk:jazzy-ci"
-
-    container: ${{ matrix.config.container }}
-
-    defaults:
-      run:
-        shell: bash
-
-    steps:
-      - uses: actions/checkout@v4
-        with:
-          path: src/fixposition_driver
-      - name: Ignore ROS1 node
-        run: |
-          touch src/fixposition_driver/fixposition_driver_ros1/COLCON_IGNORE
-          touch src/fixposition_driver/fixposition_odometry_converter_ros1/COLCON_IGNORE
-      - name: Build and Test
-        run: |
-          source /opt/ros/$ROS_DISTRO/setup.bash
-          colcon build --packages-up-to fixposition_driver_ros2 --cmake-args -DBUILD_TESTING=ON
-          colcon test --packages-up-to fixposition_driver_ros2
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
new file mode 100644
index 0000000..ee771b9
--- /dev/null
+++ b/.github/workflows/ci.yml
@@ -0,0 +1,64 @@
+name: ci
+
+on:
+    push:
+        branches: [main]
+    pull_request:
+        branches: [main]
+    workflow_dispatch:
+
+jobs:
+    # Note: job names should be unique across all workflows (e.g. to reference them as pull request checks)!
+
+    ci-noetic:
+        runs-on: ubuntu-latest
+        container:
+            image: ghcr.io/fixposition/fixposition-sdk:noetic-ci
+        defaults:
+            run:
+                shell: bash
+        steps:
+            - name: Checkout
+              uses: actions/checkout@v4
+              with:
+                    submodules: 'recursive'
+            - name: Build
+              run: |
+                  git config --global --add safe.directory ${GITHUB_WORKSPACE}
+                  ./.github/ci.sh
+
+    ci-humble:
+        runs-on: ubuntu-latest
+        container:
+            image: ghcr.io/fixposition/fixposition-sdk:humble-ci
+        defaults:
+            run:
+                shell: bash
+        steps:
+            - name: Checkout
+              uses: actions/checkout@v4
+              with:
+                    submodules: 'recursive'
+            - name: Build
+              run: |
+                  git config --global --add safe.directory ${GITHUB_WORKSPACE}
+                  ./.github/ci.sh
+
+    ci-jazzy:
+        runs-on: ubuntu-latest
+        container:
+            image: ghcr.io/fixposition/fixposition-sdk:jazzy-ci
+        defaults:
+            run:
+                shell: bash
+        steps:
+            - name: Checkout
+              uses: actions/checkout@v4
+              with:
+                    submodules: 'recursive'
+            - name: Build
+              run: |
+                  git config --global --add safe.directory ${GITHUB_WORKSPACE}
+                  ./.github/ci.sh
+
+# eof
diff --git a/.gitignore b/.gitignore
index 392ef2b..a2bb453 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,16 @@
-build/*
+build
+devel
+log
 doc_gen/*
 */doc_gen/*
-*.vscode*
+/.vscode*
+CATKIN_IGNORE
+COLCON_IGNORE
+/ros1_ws
+/ros2_ws
+/ws
+/ros1_ws
+/ros
+.catkin_tools
+core
+/tmp
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 0000000..6535c37
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,4 @@
+[submodule "fixposition-sdk"]
+	path = fixposition-sdk
+	url = https://github.com/fixposition/fixposition-sdk.git
+	branch = main
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 0000000..7bc1052
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,15 @@
+fail_fast: false
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.2.0
+    hooks:
+      #- id: check-json
+      - id: end-of-file-fixer
+      - id: trailing-whitespace
+  - repo: https://github.com/pocc/pre-commit-hooks
+    rev: v1.3.5
+    hooks:
+      - id: clang-format
+        args:
+          - "-i"
+exclude: '(.*\.svg|.*/test/data/.*|\.devcontainer/.*|\.vscode/.*)'
diff --git a/LICENSE b/LICENSE
index 24959c4..52b9c76 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2022 Fixposition AG www.fixposition.com
+Copyright (c) Fixposition AG (www.fixposition.com) and contributors
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
diff --git a/README.md b/README.md
index 87bd689..2626932 100644
--- a/README.md
+++ b/README.md
@@ -1,21 +1,17 @@
 # Fixposition ROS Driver
 
--   [ROS1 Noetic ![](./../../actions/workflows/build_test_ros.yml/badge.svg)](./../../actions/workflows/build_test_ros.yml)
--   [ROS2 Humble / Jazzy ![](./../../actions/workflows/build_test_ros2.yml/badge.svg)](./../../actions/workflows/build_test_ros2.yml)
-
-[ROS](https://www.ros.org/) (both ROS1 and ROS2) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product).
+[ROS](https://www.ros.org/) (both ROS1 and ROS2) driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product).
+![](./../../actions/workflows/ci.yml/badge.svg)
 
 ## Driver documentation
 
-For installation, usage, and more information, please refer to [Fixposition Docs: ROS Driver](https://docs.fixposition.com/fd/fixposition-ros-driver).
-
-## Fixposition ASCII messages
-
-For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs: I/O messages](https://docs.fixposition.com/fd/i-o-messages).
+For installation, usage, and more information, please refer to
+[Fixposition Docs: ROS Driver](https://docs.fixposition.com/fd/fixposition-ros-driver).
 
-## Code Documentation
+## Fixposition messages
 
-Run `doxygen Doxyfile` to generate Doxygen code documentation.
+For more information about the messages available from the Vision-RTK 2 sensor, please refer to [Fixposition Docs: I/O
+messages](https://docs.fixposition.com/fd/i-o-messages).
 
 ## License
 
diff --git a/create_ros_ws.sh b/create_ros_ws.sh
new file mode 100755
index 0000000..e60f9c1
--- /dev/null
+++ b/create_ros_ws.sh
@@ -0,0 +1,213 @@
+#!/bin/bash
+set -eEu
+set -o pipefail
+set -o errtrace
+trap 'panic' ERR
+
+SCRIPTDIR=$(dirname $(readlink -f $0))
+DEBUG=0
+
+function main
+{
+    # Get command line options
+    local help=0
+    local dev=0
+    local rosver=0
+    local checksdk=1
+    OPTERR=1
+    while getopts "hdr:vs" opt; do
+        case $opt in
+            d)
+                dev=1
+                ;;
+            r)
+                rosver=${OPTARG}
+                ;;
+            v)
+                DEBUG=1
+                ;;
+            s)
+                checksdk=0
+                ;;
+            h)
+                echo
+                echo "Create a ROS workspace for building the Fixposition ROS driver"
+                echo
+                echo "[ROS_DISTRO=...] $0 [-d] [-r 1|2] <path>"
+                echo
+                echo "    -r 1|2  -- ROS versio (default: detect from ROS_DISTRO environment variable)"
+                echo "    -s      -- Skip Fixposition SDK check"
+                echo "    -d      -- Setup for debug build (default: release)"
+                echo "    <path>  -- Path to workspace root (absolute, or relative to ${SCRIPTDIR})"
+                echo
+                exit 0
+                ;;
+            *)
+                error "Illegal option '$OPTARG'!" 1>&2
+                exit 1
+                ;;
+        esac
+    done
+    if [ ${OPTIND} -gt 1 ]; then
+        shift $(expr $OPTIND - 1)
+    fi
+
+    # Path can be relative to script directory (= fixposition_dirver repo root) or absolute
+    if [ -z "${1:-}" ]; then
+        exit_fail "Need a path"
+    fi
+    local abspath=$1
+    local relpath=
+    if [ "${abspath:0:1}" != "/" ]; then
+        relpath=${abspath}
+        abspath=${SCRIPTDIR}/${relpath}
+    fi
+    debug "abspath=${abspath} relpath=${relpath}"
+    if [ -f ${abspath} -o -d ${abspath} ]; then
+        exit_fail "Path ${abspath} already exists"
+    fi
+
+    # Check that the Fixposition SDK is present
+    if [ ${checksdk} -gt 0 -a ! -d fixposition-sdk/fpsdk_common ]; then
+        notice "Cloning Fixposition SDK"
+        if ! git submodule update --init; then
+            error "Failed cloning Fixposition SDK. Please fix that manually."
+            exit 1
+        fi
+    fi
+
+    # Get and check for ROS version
+    if [ ${rosver} -eq 0 ]; then
+        case "${ROS_DISTRO:-}" in
+            noetic)
+                rosver=1
+                ;;
+            humble|jazzy)
+                rosver=2
+                ;;
+        esac
+    fi
+    debug "rosver=${rosver}"
+    if ! [ ${rosver} -eq 1 -o ${rosver} -eq 2 ]; then
+        exit_fail "Need a (valid) ROS version"
+    fi
+
+    local res=0
+
+    # Create workspace
+    if [ ${dev} -gt 0 ]; then
+        notice "Creating debug ROS${rosver} workspace in ${abspath}"
+    else
+        notice "Creating release ROS${rosver} workspace in ${abspath}"
+    fi
+    mkdir -p ${abspath}/src
+
+    # Path for symlink
+    local srcpath=
+    if [ -n "${relpath}" ]; then
+        srcpath=$(realpath -s --relative-to=${abspath}/src ${SCRIPTDIR})
+    else
+        srcpath=${SCRIPTDIR}
+    fi
+    debug "srcpath=${srcpath}"
+
+    # Symlink source packages
+    ln -s ${srcpath}/fixposition-sdk/fpsdk_common         ${abspath}/src
+    ln -s ${srcpath}/fixposition-sdk/fpsdk_apps           ${abspath}/src
+    ln -s ${srcpath}/fixposition_driver_lib               ${abspath}/src
+    ln -s ${srcpath}/fixposition_driver_msgs              ${abspath}/src
+    ln -s ${srcpath}/rtcm_msgs                            ${abspath}/src
+    if [ ${rosver} -eq 1 ]; then
+        ln -s ${srcpath}/fixposition-sdk/fpsdk_ros1           ${abspath}/src
+        ln -s ${srcpath}/fixposition_driver_ros1              ${abspath}/src
+        ln -s ${srcpath}/fixposition_odometry_converter_ros1  ${abspath}/src
+    else
+        ln -s ${srcpath}/fixposition-sdk/fpsdk_ros2           ${abspath}/src
+        ln -s ${srcpath}/fixposition_driver_ros2              ${abspath}/src
+        ln -s ${srcpath}/fixposition_odometry_converter_ros2  ${abspath}/src
+    fi
+
+    # Initialise workspace
+    cd ${abspath}
+    if [ ${rosver} -eq 1 ]; then
+        catkin init
+        if [ ${dev} -gt 0 ]; then
+            catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug
+        else
+            catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Release
+        fi
+    else
+        if [ ${dev} -gt 0 ]; then
+            echo 'build:' > ${abspath}/colcon_defaults.yaml
+            echo '    event-handlers: [ "console_direct+" ]' >> ${abspath}/colcon_defaults.yaml
+            echo '    cmake-args: [ "-DCMAKE_BUILD_TYPE=Debug", "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON" ]' >> ${abspath}/colcon_defaults.yaml
+        else
+            echo 'build:' > ${abspath}/colcon_defaults.yaml
+            echo '#    event-handlers: [ "console_direct+" ]' >> ${abspath}/colcon_defaults.yaml
+            echo '    cmake-args: [ "-DCMAKE_BUILD_TYPE=Release", "-DCMAKE_EXPORT_COMPILE_COMMANDS=ON" ]' >> ${abspath}/colcon_defaults.yaml
+        fi
+    fi
+
+    # Happy?
+    debug "res=${res}"
+    if [ ${res} -eq 0 ]; then
+        info "Splendid! You should now be able to:"
+        info ""
+        info "    cd ${relpath}"
+        if [ ${rosver} -eq 1 ]; then
+            info "    catkin build"
+        else
+            info "    colcon build"
+        fi
+        info ""
+        exit 0
+    else
+        error "Ouch"
+        exit 1
+    fi
+
+}
+
+function exit_fail
+{
+    error "$@"
+    echo "Try '$0 -h' for help." 1>&2
+    exit 1
+}
+
+function notice
+{
+    echo -e "\033[1;37m$@\033[m" 1>&2
+}
+
+function info
+{
+    echo -e "\033[0m$@\033[m" 1>&2
+}
+
+function warning
+{
+    echo -e "\033[1;33mWarning: $@\033[m" 1>&2
+}
+
+function error
+{
+    echo -e "\033[1;31mError: $@\033[m" 1>&2
+}
+
+function debug
+{
+    if [ ${DEBUG} -gt 0 ]; then
+        echo -e "\033[0;36mDebug: $@\033[m" 1>&2
+    fi
+}
+
+function panic
+{
+    local res=$?
+    echo -e "\033[1;35mPanic at ${BASH_SOURCE[1]}:${BASH_LINENO[0]}! ${BASH_COMMAND} (res=$res)\033[m" 1>&2
+    exit $res
+}
+
+main "$@"
+exit 99
diff --git a/fixposition-sdk b/fixposition-sdk
new file mode 160000
index 0000000..6d313db
--- /dev/null
+++ b/fixposition-sdk
@@ -0,0 +1 @@
+Subproject commit 6d313dbfc343626a924811be944d81bcf3aea20b
diff --git a/fixposition_driver.code-workspace b/fixposition_driver.code-workspace
new file mode 100644
index 0000000..3d257db
--- /dev/null
+++ b/fixposition_driver.code-workspace
@@ -0,0 +1,237 @@
+{
+    "folders": [
+        { "path": ".", "name": "fixposition_driver" }
+    ],
+
+    "settings": {
+        "editor.detectIndentation":     false,
+        "editor.tabSize":               4,      // Note: only active if editor.-
+        "editor.insertSpaces":          true,   //       detectIndentation is false!
+        //"editor.trimAutoWhitespace":    true,
+        "files.trimTrailingWhitespace": true,
+
+        // Leave big brother in the dark, mainly settings from @tag:usesOnlineServices
+        "telemetry.enableTelemetry": false,
+        "gitlens.telemetry.enabled": false,
+        "redhat.telemetry.enabled": false,
+        "telemetry.telemetrylevel": "off",
+        "workbench.enableExperiments": false,
+        "workbench.settings.enableNaturalLanguageSearch": false,
+        "npm.fetchOnlinePackageInfo": false,
+
+        // Exclude files and folders from explorer (it automatically excludes .git)
+        "files.exclude": {
+            ".ackrc": true,
+            ".vscode-cache": true,
+            "**/*~": true,
+            ".vstags": true,
+            "core": true,
+            ".devcontainer/.vscode-server/**": true,
+            "**/.devcontainer/.vscode-server/**": true,
+            "**/.vscode-server/**": true
+        },
+        // Exclude things from search (this automatically uses files.exclude + what you add here)
+        "search.exclude": {
+            "output": true,
+            "tmp": true,
+            "**/ros1_ws/**": true,
+            "**/ros2_ws/**": true
+        },
+        // Exclude some things from the files watcher
+        "files.watcherExclude": {
+            "**/.git/objects/**": true,
+            "**/.git/refs/**": true,
+            "**/.git/logs/**": true,
+            "**/.git/subtree-cache/**": true,
+            "**/.git/worktrees/**": true,
+            "**/.vscode-*/**": true,
+            "**/ros1_ws/**": true,
+            "**/ros2_ws/**": true
+        },
+
+        // Microsoft C++ extension (ms-vscode.cpptools)
+        "C_Cpp.intelliSenseCachePath": "${workspaceFolder}/.vscode-cache",
+        // defaults for .vscode/c_cpp_properties.json (which makes that file optional)
+        // see .vscode/c_cpp_properties.json-example for details
+        // https://code.visualstudio.com/docs/cpp/customize-default-settings-cpp,
+        // https://code.visualstudio.com/docs/cpp/c-cpp-properties-schema-reference)
+        "C_Cpp.default.includePath":           [
+            "${workspaceFolder}/fixposition-sdk/fpsdk_common/**",
+            "${workspaceFolder}/fixposition-sdk/fpsdk_ros1/**",
+            "${workspaceFolder}/fixposition-sdk/fpsdk_ros2/**",
+            "${workspaceFolder}/fixposition-sdk/fpsdk_apps/**",
+            "${workspaceFolder}/fixposition_driver_lib/**",
+            "${workspaceFolder}/fixposition_driver_ros1/**",
+            "${workspaceFolder}/fixposition_driver_ros2/**",
+            "${workspaceFolder}/fixposition_odometry_converter_ros1/**",
+            "${workspaceFolder}/fixposition_odometry_converter_ros2/**",
+            "/opt/ros/noetic/include/**", "/opt/ros/humble/include/**", "/opt/ros/jazzy/include/**" ],
+        "C_Cpp.default.defines":               [ "FP_USE_ROS1" ],
+        //"C_Cpp.default.compileCommands":       "",
+        //"C_Cpp.default.forcedIncludes":      [ ],
+        "C_Cpp.default.intelliSenseMode":        "gcc-x64",
+        "C_Cpp.default.compilerPath":            "/usr/bin/gcc",
+        "C_Cpp.default.cStandard":               "gnu99",
+        "C_Cpp.default.cppStandard":             "gnu++17",
+        "C_Cpp.default.browse.path":           [ "${workspaceFolder}/**" ],
+        //"C_Cpp.default.configurationProvider": "b2.catkin_tools",
+        "C_Cpp.default.browse.databaseFilename": "${workspaceFolder}/.vscode-cache/browse.db",
+        //"C_Cpp.default.compileCommands":         "${workspaceFolder}/build/Debug/compile_commands.json",
+        "C_Cpp.default.browse.limitSymbolsToIncludedHeaders": false,
+        //"C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 120 }",
+        "C_Cpp.files.exclude": {
+            ".devcontainer/.vscode-server/**": true
+        },
+        "editor.formatOnSave": false,
+
+        "cmake.configureOnOpen": false,
+        "[shellscript]": {
+            "files.eol": "\n",
+            "files.trimTrailingWhitespace": false
+        },
+        "[yaml]": {
+            "editor.indentSize": 4
+        },
+        "files.associations": {
+            "memory": "cpp",
+            "cctype": "cpp",
+            "clocale": "cpp",
+            "cmath": "cpp",
+            "csignal": "cpp",
+            "cstdarg": "cpp",
+            "cstddef": "cpp",
+            "cstdio": "cpp",
+            "cstdlib": "cpp",
+            "cstring": "cpp",
+            "ctime": "cpp",
+            "cwchar": "cpp",
+            "cwctype": "cpp",
+            "any": "cpp",
+            "array": "cpp",
+            "atomic": "cpp",
+            "strstream": "cpp",
+            "bit": "cpp",
+            "*.tcc": "cpp",
+            "bitset": "cpp",
+            "chrono": "cpp",
+            "cinttypes": "cpp",
+            "codecvt": "cpp",
+            "compare": "cpp",
+            "complex": "cpp",
+            "concepts": "cpp",
+            "condition_variable": "cpp",
+            "cstdint": "cpp",
+            "deque": "cpp",
+            "forward_list": "cpp",
+            "list": "cpp",
+            "map": "cpp",
+            "set": "cpp",
+            "string": "cpp",
+            "unordered_map": "cpp",
+            "unordered_set": "cpp",
+            "vector": "cpp",
+            "exception": "cpp",
+            "algorithm": "cpp",
+            "functional": "cpp",
+            "iterator": "cpp",
+            "memory_resource": "cpp",
+            "numeric": "cpp",
+            "optional": "cpp",
+            "random": "cpp",
+            "ratio": "cpp",
+            "regex": "cpp",
+            "string_view": "cpp",
+            "system_error": "cpp",
+            "tuple": "cpp",
+            "type_traits": "cpp",
+            "utility": "cpp",
+            "fstream": "cpp",
+            "future": "cpp",
+            "initializer_list": "cpp",
+            "iomanip": "cpp",
+            "iosfwd": "cpp",
+            "iostream": "cpp",
+            "istream": "cpp",
+            "limits": "cpp",
+            "mutex": "cpp",
+            "new": "cpp",
+            "numbers": "cpp",
+            "ostream": "cpp",
+            "semaphore": "cpp",
+            "sstream": "cpp",
+            "stdexcept": "cpp",
+            "stop_token": "cpp",
+            "streambuf": "cpp",
+            "thread": "cpp",
+            "cfenv": "cpp",
+            "typeindex": "cpp",
+            "typeinfo": "cpp",
+            "valarray": "cpp",
+            "variant": "cpp",
+            "*.ipp": "cpp",
+            "ranges": "cpp",
+            "shared_mutex": "cpp",
+            "core": "cpp",
+            "splines": "cpp"
+        },
+        "workbench.remoteIndicator.showExtensionRecommendations": true,
+        "yaml.extension.recommendations": false,
+        "catkin_tools.cppStandard": "c++17",
+        "catkin_tools.defaultRosWorkspaces": [
+            "/home/fpsdk/fixposition_driver/ros1_ws",
+            "/home/fpsdk/fixposition_driver/ros2_ws",
+            "ros1_ws",
+            "ros2_ws"
+        ]
+    },
+    "extensions": {
+        "recommendations": [
+            "ms-vscode.cpptools",
+            "twxs.cmake"
+            //,"betwo.b2-catkin-tools"
+        ],
+        "unwantedRecommendations": [
+        ]
+    },
+
+    // Build and test tasks. Users can add more tasks in .vscode/tasks.json.
+    "tasks": {
+        // https://code.visualstudio.com/docs/editor/tasks
+        // https://code.visualstudio.com/docs/editor/tasks-appendix
+        "version": "2.0.0",
+
+        // Defaults for all tasks
+        "options": {
+            "cwd": "${workspaceFolder}",
+            "shell": { "executable": "/bin/bash", "args": [ "-c" ] },
+            "setupCommands": [
+                { "text": "-enable-pretty-printing", "description": "enable pretty printing", "ignoreFailures": true },
+                { "text": "handle SIGPIPE nostop noprint pass", "description": "ignore SIGPIPE", "ignoreFailures": true }
+            ]
+        },
+        "problemMatcher": "$gcc",
+        "type": "shell",
+
+        // Tasks definitions
+        "tasks": [
+            // -----------------------------------------------------------------------------------------
+            {
+                "label": "debug workspaceFolder path",
+                "group": "build",
+                "command": "echo ${workspaceFolder} && readlink -f ${workspaceFolder}"
+            }
+        ],
+
+        // Input variables
+        "inputs": [
+            // https://code.visualstudio.com/docs/editor/variables-reference#_input-variables
+        ]
+    },
+    // Launch (debugging)
+    "launch": {
+        // https://code.visualstudio.com/docs/cpp/launch-json-reference
+        "configurations": [
+        ],
+        "compounds": []
+    }
+}
diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt
index 88fb4c6..6fd0c39 100644
--- a/fixposition_driver_lib/CMakeLists.txt
+++ b/fixposition_driver_lib/CMakeLists.txt
@@ -1,58 +1,49 @@
 # GENERAL ==============================================================================================================
-cmake_minimum_required(VERSION 3.5)
-project(fixposition_driver_lib VERSION 5.0.0 LANGUAGES CXX)
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic\
-    -Wshadow -Wunused-parameter -Wformat -Wpointer-arith")
+
+cmake_minimum_required(VERSION 3.16)
+
+project(fixposition_driver_lib VERSION 8.0.0 LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror\
+    -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+    add_compile_definitions(NDEBUG)
+endif()
+
 
 # DEPENDENCIES =========================================================================================================
+
 find_package(Boost 1.65.0 REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(fpsdk_common REQUIRED)
 
 include_directories(include
   ${EIGEN3_INCLUDE_DIR}
   ${Boost_INCLUDE_DIR}
+  ${fpsdk_common_INCLUDE_DIR}
 )
 
+
 # BUILD EXECUTABLE =====================================================================================================
+
 add_library(
   ${PROJECT_NAME} SHARED
-  src/messages/fpa/corrimu.cpp
-  src/messages/fpa/eoe.cpp
-  src/messages/fpa/gnssant.cpp
-  src/messages/fpa/gnsscorr.cpp
-  src/messages/fpa/imubias.cpp
-  src/messages/fpa/llh.cpp
-  src/messages/fpa/odomenu.cpp
-  src/messages/fpa/odometry.cpp
-  src/messages/fpa/odomsh.cpp
-  src/messages/fpa/odomstatus.cpp
-  src/messages/fpa/rawimu.cpp
-  src/messages/fpa/text.cpp
-  src/messages/fpa/tf.cpp
-  src/messages/fpa/tp.cpp
-  src/messages/nmea/gpgga.cpp
-  src/messages/nmea/gpgll.cpp
-  src/messages/nmea/gngsa.cpp
-  src/messages/nmea/gpgst.cpp
-  src/messages/nmea/gxgsv.cpp
-  src/messages/nmea/gphdt.cpp
-  src/messages/nmea/gprmc.cpp
-  src/messages/nmea/gpvtg.cpp
-  src/messages/nmea/gpzda.cpp
-  src/messages/nmea/nmea.cpp
   src/fixposition_driver.cpp
   src/helper.cpp
-  src/parser.cpp
-  src/gnss_tf.cpp
+  src/params.cpp
 )
 
-target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} pthread)
+target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${fpsdk_common_LIBRARIES} pthread)
+
 
 # INSTALL ==============================================================================================================
+
 list(APPEND PACKAGE_LIBRARIES ${PROJECT_NAME})
 
 install(
diff --git a/fixposition_driver_lib/Doxyfile b/fixposition_driver_lib/Doxyfile
deleted file mode 100644
index 87bffb6..0000000
--- a/fixposition_driver_lib/Doxyfile
+++ /dev/null
@@ -1,2427 +0,0 @@
-# Doxyfile 1.8.11
-
-# This file describes the settings to be used by the documentation system
-# doxygen (www.doxygen.org) for a project.
-#
-# All text after a double hash (##) is considered a comment and is placed in
-# front of the TAG it is preceding.
-#
-# All text after a single hash (#) is considered a comment and will be ignored.
-# The format is:
-# TAG = value [value, ...]
-# For lists, items can also be appended using:
-# TAG += value [value, ...]
-# Values that contain spaces should be placed between quotes (\" \").
-
-#---------------------------------------------------------------------------
-# Project related configuration options
-#---------------------------------------------------------------------------
-
-# This tag specifies the encoding used for all characters in the config file
-# that follow. The default is UTF-8 which is also the encoding used for all text
-# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
-# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
-# for the list of possible encodings.
-# The default value is: UTF-8.
-
-DOXYFILE_ENCODING      = UTF-8
-
-# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
-# double-quotes, unless you are using Doxywizard) that should identify the
-# project for which the documentation is generated. This name is used in the
-# title of most generated pages and in a few other places.
-# The default value is: My Project.
-
-PROJECT_NAME           = "Fixposition Driver LIB"
-
-# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
-# could be handy for archiving the generated documentation or if some version
-# control system is used.
-
-PROJECT_NUMBER         =
-
-# Using the PROJECT_BRIEF tag one can provide an optional one line description
-# for a project that appears at the top of each page and should give viewer a
-# quick idea about the purpose of the project. Keep the description short.
-
-PROJECT_BRIEF          =
-
-# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
-# in the documentation. The maximum height of the logo should not exceed 55
-# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
-# the logo to the output directory.
-
-PROJECT_LOGO           =
-
-# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
-# into which the generated documentation will be written. If a relative path is
-# entered, it will be relative to the location where doxygen was started. If
-# left blank the current directory will be used.
-
-OUTPUT_DIRECTORY       = doc_gen
-
-# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
-# directories (in 2 levels) under the output directory of each output format and
-# will distribute the generated files over these directories. Enabling this
-# option can be useful when feeding doxygen a huge amount of source files, where
-# putting all generated files in the same directory would otherwise causes
-# performance problems for the file system.
-# The default value is: NO.
-
-CREATE_SUBDIRS         = NO
-
-# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
-# characters to appear in the names of generated files. If set to NO, non-ASCII
-# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
-# U+3044.
-# The default value is: NO.
-
-ALLOW_UNICODE_NAMES    = NO
-
-# The OUTPUT_LANGUAGE tag is used to specify the language in which all
-# documentation generated by doxygen is written. Doxygen will use this
-# information to generate all constant output in the proper language.
-# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
-# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
-# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
-# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
-# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
-# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
-# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
-# Ukrainian and Vietnamese.
-# The default value is: English.
-
-OUTPUT_LANGUAGE        = English
-
-# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
-# descriptions after the members that are listed in the file and class
-# documentation (similar to Javadoc). Set to NO to disable this.
-# The default value is: YES.
-
-BRIEF_MEMBER_DESC      = YES
-
-# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
-# description of a member or function before the detailed description
-#
-# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
-# brief descriptions will be completely suppressed.
-# The default value is: YES.
-
-REPEAT_BRIEF           = YES
-
-# This tag implements a quasi-intelligent brief description abbreviator that is
-# used to form the text in various listings. Each string in this list, if found
-# as the leading text of the brief description, will be stripped from the text
-# and the result, after processing the whole list, is used as the annotated
-# text. Otherwise, the brief description is used as-is. If left blank, the
-# following values are used ($name is automatically replaced with the name of
-# the entity):The $name class, The $name widget, The $name file, is, provides,
-# specifies, contains, represents, a, an and the.
-
-ABBREVIATE_BRIEF       =
-
-# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
-# doxygen will generate a detailed section even if there is only a brief
-# description.
-# The default value is: NO.
-
-ALWAYS_DETAILED_SEC    = NO
-
-# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
-# inherited members of a class in the documentation of that class as if those
-# members were ordinary class members. Constructors, destructors and assignment
-# operators of the base classes will not be shown.
-# The default value is: NO.
-
-INLINE_INHERITED_MEMB  = NO
-
-# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
-# before files name in the file list and in the header files. If set to NO the
-# shortest path that makes the file name unique will be used
-# The default value is: YES.
-
-FULL_PATH_NAMES        = YES
-
-# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
-# Stripping is only done if one of the specified strings matches the left-hand
-# part of the path. The tag can be used to show relative paths in the file list.
-# If left blank the directory from which doxygen is run is used as the path to
-# strip.
-#
-# Note that you can specify absolute paths here, but also relative paths, which
-# will be relative from the directory where doxygen is started.
-# This tag requires that the tag FULL_PATH_NAMES is set to YES.
-
-STRIP_FROM_PATH        =
-
-# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
-# path mentioned in the documentation of a class, which tells the reader which
-# header file to include in order to use a class. If left blank only the name of
-# the header file containing the class definition is used. Otherwise one should
-# specify the list of include paths that are normally passed to the compiler
-# using the -I flag.
-
-STRIP_FROM_INC_PATH    =
-
-# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
-# less readable) file names. This can be useful is your file systems doesn't
-# support long names like on DOS, Mac, or CD-ROM.
-# The default value is: NO.
-
-SHORT_NAMES            = NO
-
-# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
-# first line (until the first dot) of a Javadoc-style comment as the brief
-# description. If set to NO, the Javadoc-style will behave just like regular Qt-
-# style comments (thus requiring an explicit @brief command for a brief
-# description.)
-# The default value is: NO.
-
-JAVADOC_AUTOBRIEF      = NO
-
-# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
-# line (until the first dot) of a Qt-style comment as the brief description. If
-# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
-# requiring an explicit \brief command for a brief description.)
-# The default value is: NO.
-
-QT_AUTOBRIEF           = NO
-
-# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
-# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
-# a brief description. This used to be the default behavior. The new default is
-# to treat a multi-line C++ comment block as a detailed description. Set this
-# tag to YES if you prefer the old behavior instead.
-#
-# Note that setting this tag to YES also means that rational rose comments are
-# not recognized any more.
-# The default value is: NO.
-
-MULTILINE_CPP_IS_BRIEF = NO
-
-# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
-# documentation from any documented member that it re-implements.
-# The default value is: YES.
-
-INHERIT_DOCS           = YES
-
-# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
-# page for each member. If set to NO, the documentation of a member will be part
-# of the file/class/namespace that contains it.
-# The default value is: NO.
-
-SEPARATE_MEMBER_PAGES  = NO
-
-# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
-# uses this value to replace tabs by spaces in code fragments.
-# Minimum value: 1, maximum value: 16, default value: 4.
-
-TAB_SIZE               = 4
-
-# This tag can be used to specify a number of aliases that act as commands in
-# the documentation. An alias has the form:
-# name=value
-# For example adding
-# "sideeffect=@par Side Effects:\n"
-# will allow you to put the command \sideeffect (or @sideeffect) in the
-# documentation, which will result in a user-defined paragraph with heading
-# "Side Effects:". You can put \n's in the value part of an alias to insert
-# newlines.
-
-ALIASES                =
-
-# This tag can be used to specify a number of word-keyword mappings (TCL only).
-# A mapping has the form "name=value". For example adding "class=itcl::class"
-# will allow you to use the command class in the itcl::class meaning.
-
-TCL_SUBST              =
-
-# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
-# only. Doxygen will then generate output that is more tailored for C. For
-# instance, some of the names that are used will be different. The list of all
-# members will be omitted, etc.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_FOR_C  = NO
-
-# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
-# Python sources only. Doxygen will then generate output that is more tailored
-# for that language. For instance, namespaces will be presented as packages,
-# qualified scopes will look different, etc.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_JAVA   = NO
-
-# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
-# sources. Doxygen will then generate output that is tailored for Fortran.
-# The default value is: NO.
-
-OPTIMIZE_FOR_FORTRAN   = NO
-
-# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
-# sources. Doxygen will then generate output that is tailored for VHDL.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_VHDL   = NO
-
-# Doxygen selects the parser to use depending on the extension of the files it
-# parses. With this tag you can assign which parser to use for a given
-# extension. Doxygen has a built-in mapping, but you can override or extend it
-# using this tag. The format is ext=language, where ext is a file extension, and
-# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
-# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
-# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
-# Fortran. In the later case the parser tries to guess whether the code is fixed
-# or free formatted code, this is the default for Fortran type files), VHDL. For
-# instance to make doxygen treat .inc files as Fortran files (default is PHP),
-# and .f files as C (default is Fortran), use: inc=Fortran f=C.
-#
-# Note: For files without extension you can use no_extension as a placeholder.
-#
-# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
-# the files are not read by doxygen.
-
-EXTENSION_MAPPING      =
-
-# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
-# according to the Markdown format, which allows for more readable
-# documentation. See http://daringfireball.net/projects/markdown/ for details.
-# The output of markdown processing is further processed by doxygen, so you can
-# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
-# case of backward compatibilities issues.
-# The default value is: YES.
-
-MARKDOWN_SUPPORT       = YES
-
-# When enabled doxygen tries to link words that correspond to documented
-# classes, or namespaces to their corresponding documentation. Such a link can
-# be prevented in individual cases by putting a % sign in front of the word or
-# globally by setting AUTOLINK_SUPPORT to NO.
-# The default value is: YES.
-
-AUTOLINK_SUPPORT       = YES
-
-# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
-# to include (a tag file for) the STL sources as input, then you should set this
-# tag to YES in order to let doxygen match functions declarations and
-# definitions whose arguments contain STL classes (e.g. func(std::string);
-# versus func(std::string) {}). This also make the inheritance and collaboration
-# diagrams that involve STL classes more complete and accurate.
-# The default value is: NO.
-
-BUILTIN_STL_SUPPORT    = NO
-
-# If you use Microsoft's C++/CLI language, you should set this option to YES to
-# enable parsing support.
-# The default value is: NO.
-
-CPP_CLI_SUPPORT        = NO
-
-# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
-# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
-# will parse them like normal C++ but will assume all classes use public instead
-# of private inheritance when no explicit protection keyword is present.
-# The default value is: NO.
-
-SIP_SUPPORT            = NO
-
-# For Microsoft's IDL there are propget and propput attributes to indicate
-# getter and setter methods for a property. Setting this option to YES will make
-# doxygen to replace the get and set methods by a property in the documentation.
-# This will only work if the methods are indeed getting or setting a simple
-# type. If this is not the case, or you want to show the methods anyway, you
-# should set this option to NO.
-# The default value is: YES.
-
-IDL_PROPERTY_SUPPORT   = YES
-
-# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
-# tag is set to YES then doxygen will reuse the documentation of the first
-# member in the group (if any) for the other members of the group. By default
-# all members of a group must be documented explicitly.
-# The default value is: NO.
-
-DISTRIBUTE_GROUP_DOC   = NO
-
-# If one adds a struct or class to a group and this option is enabled, then also
-# any nested class or struct is added to the same group. By default this option
-# is disabled and one has to add nested compounds explicitly via \ingroup.
-# The default value is: NO.
-
-GROUP_NESTED_COMPOUNDS = NO
-
-# Set the SUBGROUPING tag to YES to allow class member groups of the same type
-# (for instance a group of public functions) to be put as a subgroup of that
-# type (e.g. under the Public Functions section). Set it to NO to prevent
-# subgrouping. Alternatively, this can be done per class using the
-# \nosubgrouping command.
-# The default value is: YES.
-
-SUBGROUPING            = YES
-
-# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
-# are shown inside the group in which they are included (e.g. using \ingroup)
-# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
-# and RTF).
-#
-# Note that this feature does not work in combination with
-# SEPARATE_MEMBER_PAGES.
-# The default value is: NO.
-
-INLINE_GROUPED_CLASSES = NO
-
-# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
-# with only public data fields or simple typedef fields will be shown inline in
-# the documentation of the scope in which they are defined (i.e. file,
-# namespace, or group documentation), provided this scope is documented. If set
-# to NO, structs, classes, and unions are shown on a separate page (for HTML and
-# Man pages) or section (for LaTeX and RTF).
-# The default value is: NO.
-
-INLINE_SIMPLE_STRUCTS  = NO
-
-# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
-# enum is documented as struct, union, or enum with the name of the typedef. So
-# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
-# with name TypeT. When disabled the typedef will appear as a member of a file,
-# namespace, or class. And the struct will be named TypeS. This can typically be
-# useful for C code in case the coding convention dictates that all compound
-# types are typedef'ed and only the typedef is referenced, never the tag name.
-# The default value is: NO.
-
-TYPEDEF_HIDES_STRUCT   = NO
-
-# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
-# cache is used to resolve symbols given their name and scope. Since this can be
-# an expensive process and often the same symbol appears multiple times in the
-# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
-# doxygen will become slower. If the cache is too large, memory is wasted. The
-# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
-# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
-# symbols. At the end of a run doxygen will report the cache usage and suggest
-# the optimal cache size from a speed point of view.
-# Minimum value: 0, maximum value: 9, default value: 0.
-
-LOOKUP_CACHE_SIZE      = 0
-
-#---------------------------------------------------------------------------
-# Build related configuration options
-#---------------------------------------------------------------------------
-
-# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
-# documentation are documented, even if no documentation was available. Private
-# class members and static file members will be hidden unless the
-# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
-# Note: This will also disable the warnings about undocumented members that are
-# normally produced when WARNINGS is set to YES.
-# The default value is: NO.
-
-EXTRACT_ALL            =YES
-
-# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
-# be included in the documentation.
-# The default value is: NO.
-
-EXTRACT_PRIVATE        = YES
-
-# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
-# scope will be included in the documentation.
-# The default value is: NO.
-
-EXTRACT_PACKAGE        = NO
-
-# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
-# included in the documentation.
-# The default value is: NO.
-
-EXTRACT_STATIC         = YES
-
-# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
-# locally in source files will be included in the documentation. If set to NO,
-# only classes defined in header files are included. Does not have any effect
-# for Java sources.
-# The default value is: YES.
-
-EXTRACT_LOCAL_CLASSES  = YES
-
-# This flag is only useful for Objective-C code. If set to YES, local methods,
-# which are defined in the implementation section but not in the interface are
-# included in the documentation. If set to NO, only methods in the interface are
-# included.
-# The default value is: NO.
-
-EXTRACT_LOCAL_METHODS  = NO
-
-# If this flag is set to YES, the members of anonymous namespaces will be
-# extracted and appear in the documentation as a namespace called
-# 'anonymous_namespace{file}', where file will be replaced with the base name of
-# the file that contains the anonymous namespace. By default anonymous namespace
-# are hidden.
-# The default value is: NO.
-
-EXTRACT_ANON_NSPACES   = NO
-
-# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
-# undocumented members inside documented classes or files. If set to NO these
-# members will be included in the various overviews, but no documentation
-# section is generated. This option has no effect if EXTRACT_ALL is enabled.
-# The default value is: NO.
-
-HIDE_UNDOC_MEMBERS     = NO
-
-# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
-# undocumented classes that are normally visible in the class hierarchy. If set
-# to NO, these classes will be included in the various overviews. This option
-# has no effect if EXTRACT_ALL is enabled.
-# The default value is: NO.
-
-HIDE_UNDOC_CLASSES     = NO
-
-# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
-# (class|struct|union) declarations. If set to NO, these declarations will be
-# included in the documentation.
-# The default value is: NO.
-
-HIDE_FRIEND_COMPOUNDS  = NO
-
-# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
-# documentation blocks found inside the body of a function. If set to NO, these
-# blocks will be appended to the function's detailed documentation block.
-# The default value is: NO.
-
-HIDE_IN_BODY_DOCS      = NO
-
-# The INTERNAL_DOCS tag determines if documentation that is typed after a
-# \internal command is included. If the tag is set to NO then the documentation
-# will be excluded. Set it to YES to include the internal documentation.
-# The default value is: NO.
-
-INTERNAL_DOCS          = NO
-
-# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
-# names in lower-case letters. If set to YES, upper-case letters are also
-# allowed. This is useful if you have classes or files whose names only differ
-# in case and if your file system supports case sensitive file names. Windows
-# and Mac users are advised to set this option to NO.
-# The default value is: system dependent.
-
-CASE_SENSE_NAMES       = YES
-
-# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
-# their full class and namespace scopes in the documentation. If set to YES, the
-# scope will be hidden.
-# The default value is: NO.
-
-HIDE_SCOPE_NAMES       = NO
-
-# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
-# append additional text to a page's title, such as Class Reference. If set to
-# YES the compound reference will be hidden.
-# The default value is: NO.
-
-HIDE_COMPOUND_REFERENCE= NO
-
-# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
-# the files that are included by a file in the documentation of that file.
-# The default value is: YES.
-
-SHOW_INCLUDE_FILES     = YES
-
-# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
-# grouped member an include statement to the documentation, telling the reader
-# which file to include in order to use the member.
-# The default value is: NO.
-
-SHOW_GROUPED_MEMB_INC  = NO
-
-# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
-# files with double quotes in the documentation rather than with sharp brackets.
-# The default value is: NO.
-
-FORCE_LOCAL_INCLUDES   = NO
-
-# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
-# documentation for inline members.
-# The default value is: YES.
-
-INLINE_INFO            = YES
-
-# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
-# (detailed) documentation of file and class members alphabetically by member
-# name. If set to NO, the members will appear in declaration order.
-# The default value is: YES.
-
-SORT_MEMBER_DOCS       = YES
-
-# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
-# descriptions of file, namespace and class members alphabetically by member
-# name. If set to NO, the members will appear in declaration order. Note that
-# this will also influence the order of the classes in the class list.
-# The default value is: NO.
-
-SORT_BRIEF_DOCS        = NO
-
-# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
-# (brief and detailed) documentation of class members so that constructors and
-# destructors are listed first. If set to NO the constructors will appear in the
-# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
-# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
-# member documentation.
-# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
-# detailed member documentation.
-# The default value is: NO.
-
-SORT_MEMBERS_CTORS_1ST = NO
-
-# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
-# of group names into alphabetical order. If set to NO the group names will
-# appear in their defined order.
-# The default value is: NO.
-
-SORT_GROUP_NAMES       = NO
-
-# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
-# fully-qualified names, including namespaces. If set to NO, the class list will
-# be sorted only by class name, not including the namespace part.
-# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
-# Note: This option applies only to the class list, not to the alphabetical
-# list.
-# The default value is: NO.
-
-SORT_BY_SCOPE_NAME     = NO
-
-# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
-# type resolution of all parameters of a function it will reject a match between
-# the prototype and the implementation of a member function even if there is
-# only one candidate or it is obvious which candidate to choose by doing a
-# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
-# accept a match between prototype and implementation in such cases.
-# The default value is: NO.
-
-STRICT_PROTO_MATCHING  = NO
-
-# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
-# list. This list is created by putting \todo commands in the documentation.
-# The default value is: YES.
-
-GENERATE_TODOLIST      = YES
-
-# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
-# list. This list is created by putting \test commands in the documentation.
-# The default value is: YES.
-
-GENERATE_TESTLIST      = YES
-
-# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
-# list. This list is created by putting \bug commands in the documentation.
-# The default value is: YES.
-
-GENERATE_BUGLIST       = YES
-
-# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
-# the deprecated list. This list is created by putting \deprecated commands in
-# the documentation.
-# The default value is: YES.
-
-GENERATE_DEPRECATEDLIST= YES
-
-# The ENABLED_SECTIONS tag can be used to enable conditional documentation
-# sections, marked by \if <section_label> ... \endif and \cond <section_label>
-# ... \endcond blocks.
-
-ENABLED_SECTIONS       =
-
-# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
-# initial value of a variable or macro / define can have for it to appear in the
-# documentation. If the initializer consists of more lines than specified here
-# it will be hidden. Use a value of 0 to hide initializers completely. The
-# appearance of the value of individual variables and macros / defines can be
-# controlled using \showinitializer or \hideinitializer command in the
-# documentation regardless of this setting.
-# Minimum value: 0, maximum value: 10000, default value: 30.
-
-MAX_INITIALIZER_LINES  = 30
-
-# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
-# the bottom of the documentation of classes and structs. If set to YES, the
-# list will mention the files that were used to generate the documentation.
-# The default value is: YES.
-
-SHOW_USED_FILES        = YES
-
-# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
-# will remove the Files entry from the Quick Index and from the Folder Tree View
-# (if specified).
-# The default value is: YES.
-
-SHOW_FILES             = YES
-
-# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
-# page. This will remove the Namespaces entry from the Quick Index and from the
-# Folder Tree View (if specified).
-# The default value is: YES.
-
-SHOW_NAMESPACES        = YES
-
-# The FILE_VERSION_FILTER tag can be used to specify a program or script that
-# doxygen should invoke to get the current version for each file (typically from
-# the version control system). Doxygen will invoke the program by executing (via
-# popen()) the command command input-file, where command is the value of the
-# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
-# by doxygen. Whatever the program writes to standard output is used as the file
-# version. For an example see the documentation.
-
-FILE_VERSION_FILTER    =
-
-# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
-# by doxygen. The layout file controls the global structure of the generated
-# output files in an output format independent way. To create the layout file
-# that represents doxygen's defaults, run doxygen with the -l option. You can
-# optionally specify a file name after the option, if omitted DoxygenLayout.xml
-# will be used as the name of the layout file.
-#
-# Note that if you run doxygen from a directory containing a file called
-# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
-# tag is left empty.
-
-LAYOUT_FILE            =
-
-# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
-# the reference definitions. This must be a list of .bib files. The .bib
-# extension is automatically appended if omitted. This requires the bibtex tool
-# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
-# For LaTeX the style of the bibliography can be controlled using
-# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
-# search path. See also \cite for info how to create references.
-
-CITE_BIB_FILES         =
-
-#---------------------------------------------------------------------------
-# Configuration options related to warning and progress messages
-#---------------------------------------------------------------------------
-
-# The QUIET tag can be used to turn on/off the messages that are generated to
-# standard output by doxygen. If QUIET is set to YES this implies that the
-# messages are off.
-# The default value is: NO.
-
-QUIET                  = NO
-
-# The WARNINGS tag can be used to turn on/off the warning messages that are
-# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
-# this implies that the warnings are on.
-#
-# Tip: Turn warnings on while writing the documentation.
-# The default value is: YES.
-
-WARNINGS               = YES
-
-# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
-# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
-# will automatically be disabled.
-# The default value is: YES.
-
-WARN_IF_UNDOCUMENTED   = YES
-
-# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
-# potential errors in the documentation, such as not documenting some parameters
-# in a documented function, or documenting parameters that don't exist or using
-# markup commands wrongly.
-# The default value is: YES.
-
-WARN_IF_DOC_ERROR      = YES
-
-# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
-# are documented, but have no documentation for their parameters or return
-# value. If set to NO, doxygen will only warn about wrong or incomplete
-# parameter documentation, but not about the absence of documentation.
-# The default value is: NO.
-
-WARN_NO_PARAMDOC       = NO
-
-# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
-# a warning is encountered.
-# The default value is: NO.
-
-WARN_AS_ERROR          = NO
-
-# The WARN_FORMAT tag determines the format of the warning messages that doxygen
-# can produce. The string should contain the $file, $line, and $text tags, which
-# will be replaced by the file and line number from which the warning originated
-# and the warning text. Optionally the format may contain $version, which will
-# be replaced by the version of the file (if it could be obtained via
-# FILE_VERSION_FILTER)
-# The default value is: $file:$line: $text.
-
-WARN_FORMAT            = "$file:$line: $text"
-
-# The WARN_LOGFILE tag can be used to specify a file to which warning and error
-# messages should be written. If left blank the output is written to standard
-# error (stderr).
-
-WARN_LOGFILE           =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the input files
-#---------------------------------------------------------------------------
-
-# The INPUT tag is used to specify the files and/or directories that contain
-# documented source files. You may enter file names like myfile.cpp or
-# directories like /usr/src/myproject. Separate the files or directories with
-# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
-# Note: If this tag is empty the current directory is searched.
-
-INPUT                  = include src submodules README.md
-
-# This tag can be used to specify the character encoding of the source files
-# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
-# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
-# documentation (see: http://www.gnu.org/software/libiconv) for the list of
-# possible encodings.
-# The default value is: UTF-8.
-
-INPUT_ENCODING         = UTF-8
-
-# If the value of the INPUT tag contains directories, you can use the
-# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
-# *.h) to filter out the source-files in the directories.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# read by doxygen.
-#
-# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
-# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
-# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
-# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl,
-# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js.
-
-FILE_PATTERNS          =
-
-# The RECURSIVE tag can be used to specify whether or not subdirectories should
-# be searched for input files as well.
-# The default value is: NO.
-
-RECURSIVE              = YES
-
-# The EXCLUDE tag can be used to specify files and/or directories that should be
-# excluded from the INPUT source files. This way you can easily exclude a
-# subdirectory from a directory tree whose root is specified with the INPUT tag.
-#
-# Note that relative paths are relative to the directory from which doxygen is
-# run.
-
-EXCLUDE                =
-
-# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
-# directories that are symbolic links (a Unix file system feature) are excluded
-# from the input.
-# The default value is: NO.
-
-EXCLUDE_SYMLINKS       = NO
-
-# If the value of the INPUT tag contains directories, you can use the
-# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
-# certain files from those directories.
-#
-# Note that the wildcards are matched against the file with absolute path, so to
-# exclude all test directories for example use the pattern */test/*
-
-EXCLUDE_PATTERNS       =
-
-# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
-# (namespaces, classes, functions, etc.) that should be excluded from the
-# output. The symbol name can be a fully qualified name, a word, or if the
-# wildcard * is used, a substring. Examples: ANamespace, AClass,
-# AClass::ANamespace, ANamespace::*Test
-#
-# Note that the wildcards are matched against the file with absolute path, so to
-# exclude all test directories use the pattern */test/*
-
-EXCLUDE_SYMBOLS        =
-
-# The EXAMPLE_PATH tag can be used to specify one or more files or directories
-# that contain example code fragments that are included (see the \include
-# command).
-
-EXAMPLE_PATH           =
-
-# If the value of the EXAMPLE_PATH tag contains directories, you can use the
-# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
-# *.h) to filter out the source-files in the directories. If left blank all
-# files are included.
-
-EXAMPLE_PATTERNS       =
-
-# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
-# searched for input files to be used with the \include or \dontinclude commands
-# irrespective of the value of the RECURSIVE tag.
-# The default value is: NO.
-
-EXAMPLE_RECURSIVE      = NO
-
-# The IMAGE_PATH tag can be used to specify one or more files or directories
-# that contain images that are to be included in the documentation (see the
-# \image command).
-
-IMAGE_PATH             =
-
-# The INPUT_FILTER tag can be used to specify a program that doxygen should
-# invoke to filter for each input file. Doxygen will invoke the filter program
-# by executing (via popen()) the command:
-#
-# <filter> <input-file>
-#
-# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
-# name of an input file. Doxygen will then use the output that the filter
-# program writes to standard output. If FILTER_PATTERNS is specified, this tag
-# will be ignored.
-#
-# Note that the filter must not add or remove lines; it is applied before the
-# code is scanned, but not when the output code is generated. If lines are added
-# or removed, the anchors will not be placed correctly.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# properly processed by doxygen.
-
-INPUT_FILTER           =
-
-# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
-# basis. Doxygen will compare the file name with each pattern and apply the
-# filter if there is a match. The filters are a list of the form: pattern=filter
-# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
-# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
-# patterns match the file name, INPUT_FILTER is applied.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# properly processed by doxygen.
-
-FILTER_PATTERNS        =
-
-# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
-# INPUT_FILTER) will also be used to filter the input files that are used for
-# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
-# The default value is: NO.
-
-FILTER_SOURCE_FILES    = NO
-
-# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
-# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
-# it is also possible to disable source filtering for a specific pattern using
-# *.ext= (so without naming a filter).
-# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
-
-FILTER_SOURCE_PATTERNS =
-
-# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
-# is part of the input, its contents will be placed on the main page
-# (index.html). This can be useful if you have a project on for instance GitHub
-# and want to reuse the introduction page also for the doxygen output.
-
-USE_MDFILE_AS_MAINPAGE = README.md
-
-#---------------------------------------------------------------------------
-# Configuration options related to source browsing
-#---------------------------------------------------------------------------
-
-# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
-# generated. Documented entities will be cross-referenced with these sources.
-#
-# Note: To get rid of all source code in the generated output, make sure that
-# also VERBATIM_HEADERS is set to NO.
-# The default value is: NO.
-
-SOURCE_BROWSER         = NO
-
-# Setting the INLINE_SOURCES tag to YES will include the body of functions,
-# classes and enums directly into the documentation.
-# The default value is: NO.
-
-INLINE_SOURCES         = NO
-
-# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
-# special comment blocks from generated source code fragments. Normal C, C++ and
-# Fortran comments will always remain visible.
-# The default value is: YES.
-
-STRIP_CODE_COMMENTS    = YES
-
-# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
-# function all documented functions referencing it will be listed.
-# The default value is: NO.
-
-REFERENCED_BY_RELATION = NO
-
-# If the REFERENCES_RELATION tag is set to YES then for each documented function
-# all documented entities called/used by that function will be listed.
-# The default value is: NO.
-
-REFERENCES_RELATION    = NO
-
-# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
-# to YES then the hyperlinks from functions in REFERENCES_RELATION and
-# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
-# link to the documentation.
-# The default value is: YES.
-
-REFERENCES_LINK_SOURCE = YES
-
-# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
-# source code will show a tooltip with additional information such as prototype,
-# brief description and links to the definition and documentation. Since this
-# will make the HTML file larger and loading of large files a bit slower, you
-# can opt to disable this feature.
-# The default value is: YES.
-# This tag requires that the tag SOURCE_BROWSER is set to YES.
-
-SOURCE_TOOLTIPS        = YES
-
-# If the USE_HTAGS tag is set to YES then the references to source code will
-# point to the HTML generated by the htags(1) tool instead of doxygen built-in
-# source browser. The htags tool is part of GNU's global source tagging system
-# (see http://www.gnu.org/software/global/global.html). You will need version
-# 4.8.6 or higher.
-#
-# To use it do the following:
-# - Install the latest version of global
-# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
-# - Make sure the INPUT points to the root of the source tree
-# - Run doxygen as normal
-#
-# Doxygen will invoke htags (and that will in turn invoke gtags), so these
-# tools must be available from the command line (i.e. in the search path).
-#
-# The result: instead of the source browser generated by doxygen, the links to
-# source code will now point to the output of htags.
-# The default value is: NO.
-# This tag requires that the tag SOURCE_BROWSER is set to YES.
-
-USE_HTAGS              = NO
-
-# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
-# verbatim copy of the header file for each class for which an include is
-# specified. Set to NO to disable this.
-# See also: Section \class.
-# The default value is: YES.
-
-VERBATIM_HEADERS       = YES
-
-# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
-# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
-# cost of reduced performance. This can be particularly helpful with template
-# rich C++ code for which doxygen's built-in parser lacks the necessary type
-# information.
-# Note: The availability of this option depends on whether or not doxygen was
-# generated with the -Duse-libclang=ON option for CMake.
-# The default value is: NO.
-
-CLANG_ASSISTED_PARSING = NO
-
-# If clang assisted parsing is enabled you can provide the compiler with command
-# line options that you would normally use when invoking the compiler. Note that
-# the include paths will already be set by doxygen for the files and directories
-# specified with INPUT and INCLUDE_PATH.
-# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
-
-CLANG_OPTIONS          =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the alphabetical class index
-#---------------------------------------------------------------------------
-
-# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
-# compounds will be generated. Enable this if the project contains a lot of
-# classes, structs, unions or interfaces.
-# The default value is: YES.
-
-ALPHABETICAL_INDEX     = YES
-
-# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
-# which the alphabetical index list will be split.
-# Minimum value: 1, maximum value: 20, default value: 5.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-COLS_IN_ALPHA_INDEX    = 5
-
-# In case all classes in a project start with a common prefix, all classes will
-# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
-# can be used to specify a prefix (or a list of prefixes) that should be ignored
-# while generating the index headers.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-IGNORE_PREFIX          =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the HTML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
-# The default value is: YES.
-
-GENERATE_HTML          = YES
-
-# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: html.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_OUTPUT            = html
-
-# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
-# generated HTML page (for example: .htm, .php, .asp).
-# The default value is: .html.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_FILE_EXTENSION    = .html
-
-# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
-# each generated HTML page. If the tag is left blank doxygen will generate a
-# standard header.
-#
-# To get valid HTML the header file that includes any scripts and style sheets
-# that doxygen needs, which is dependent on the configuration options used (e.g.
-# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
-# default header using
-# doxygen -w html new_header.html new_footer.html new_stylesheet.css
-# YourConfigFile
-# and then modify the file new_header.html. See also section "Doxygen usage"
-# for information on how to generate the default header that doxygen normally
-# uses.
-# Note: The header is subject to change so you typically have to regenerate the
-# default header when upgrading to a newer version of doxygen. For a description
-# of the possible markers and block names see the documentation.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_HEADER            =
-
-# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
-# generated HTML page. If the tag is left blank doxygen will generate a standard
-# footer. See HTML_HEADER for more information on how to generate a default
-# footer and what special commands can be used inside the footer. See also
-# section "Doxygen usage" for information on how to generate the default footer
-# that doxygen normally uses.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_FOOTER            =
-
-# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
-# sheet that is used by each HTML page. It can be used to fine-tune the look of
-# the HTML output. If left blank doxygen will generate a default style sheet.
-# See also section "Doxygen usage" for information on how to generate the style
-# sheet that doxygen normally uses.
-# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
-# it is more robust and this tag (HTML_STYLESHEET) will in the future become
-# obsolete.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_STYLESHEET        =
-
-# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
-# cascading style sheets that are included after the standard style sheets
-# created by doxygen. Using this option one can overrule certain style aspects.
-# This is preferred over using HTML_STYLESHEET since it does not replace the
-# standard style sheet and is therefore more robust against future updates.
-# Doxygen will copy the style sheet files to the output directory.
-# Note: The order of the extra style sheet files is of importance (e.g. the last
-# style sheet in the list overrules the setting of the previous ones in the
-# list). For an example see the documentation.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_EXTRA_STYLESHEET  =
-
-# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
-# other source files which should be copied to the HTML output directory. Note
-# that these files will be copied to the base HTML output directory. Use the
-# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
-# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
-# files will be copied as-is; there are no commands or markers available.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_EXTRA_FILES       =
-
-# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
-# will adjust the colors in the style sheet and background images according to
-# this color. Hue is specified as an angle on a colorwheel, see
-# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
-# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
-# purple, and 360 is red again.
-# Minimum value: 0, maximum value: 359, default value: 220.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_HUE    = 220
-
-# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
-# in the HTML output. For a value of 0 the output will use grayscales only. A
-# value of 255 will produce the most vivid colors.
-# Minimum value: 0, maximum value: 255, default value: 100.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_SAT    = 100
-
-# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
-# luminance component of the colors in the HTML output. Values below 100
-# gradually make the output lighter, whereas values above 100 make the output
-# darker. The value divided by 100 is the actual gamma applied, so 80 represents
-# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
-# change the gamma.
-# Minimum value: 40, maximum value: 240, default value: 80.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_GAMMA  = 80
-
-# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
-# page will contain the date and time when the page was generated. Setting this
-# to YES can help to show when doxygen was last run and thus if the
-# documentation is up to date.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_TIMESTAMP         = NO
-
-# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
-# documentation will contain sections that can be hidden and shown after the
-# page has loaded.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_DYNAMIC_SECTIONS  = NO
-
-# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
-# shown in the various tree structured indices initially; the user can expand
-# and collapse entries dynamically later on. Doxygen will expand the tree to
-# such a level that at most the specified number of entries are visible (unless
-# a fully collapsed tree already exceeds this amount). So setting the number of
-# entries 1 will produce a full collapsed tree by default. 0 is a special value
-# representing an infinite number of entries and will result in a full expanded
-# tree by default.
-# Minimum value: 0, maximum value: 9999, default value: 100.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_INDEX_NUM_ENTRIES = 100
-
-# If the GENERATE_DOCSET tag is set to YES, additional index files will be
-# generated that can be used as input for Apple's Xcode 3 integrated development
-# environment (see: http://developer.apple.com/tools/xcode/), introduced with
-# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
-# Makefile in the HTML output directory. Running make will produce the docset in
-# that directory and running make install will install the docset in
-# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
-# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
-# for more information.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_DOCSET        = NO
-
-# This tag determines the name of the docset feed. A documentation feed provides
-# an umbrella under which multiple documentation sets from a single provider
-# (such as a company or product suite) can be grouped.
-# The default value is: Doxygen generated docs.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_FEEDNAME        = "Doxygen generated docs"
-
-# This tag specifies a string that should uniquely identify the documentation
-# set bundle. This should be a reverse domain-name style string, e.g.
-# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_BUNDLE_ID       = org.doxygen.Project
-
-# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
-# the documentation publisher. This should be a reverse domain-name style
-# string, e.g. com.mycompany.MyDocSet.documentation.
-# The default value is: org.doxygen.Publisher.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
-
-# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
-# The default value is: Publisher.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_PUBLISHER_NAME  = Publisher
-
-# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
-# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
-# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
-# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
-# Windows.
-#
-# The HTML Help Workshop contains a compiler that can convert all HTML output
-# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
-# files are now used as the Windows 98 help format, and will replace the old
-# Windows help format (.hlp) on all Windows platforms in the future. Compressed
-# HTML files also contain an index, a table of contents, and you can search for
-# words in the documentation. The HTML workshop also contains a viewer for
-# compressed HTML files.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_HTMLHELP      = NO
-
-# The CHM_FILE tag can be used to specify the file name of the resulting .chm
-# file. You can add a path in front of the file if the result should not be
-# written to the html output directory.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-CHM_FILE               =
-
-# The HHC_LOCATION tag can be used to specify the location (absolute path
-# including file name) of the HTML help compiler (hhc.exe). If non-empty,
-# doxygen will try to run the HTML help compiler on the generated index.hhp.
-# The file has to be specified with full path.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-HHC_LOCATION           =
-
-# The GENERATE_CHI flag controls if a separate .chi index file is generated
-# (YES) or that it should be included in the master .chm file (NO).
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-GENERATE_CHI           = NO
-
-# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
-# and project file content.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-CHM_INDEX_ENCODING     =
-
-# The BINARY_TOC flag controls whether a binary table of contents is generated
-# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
-# enables the Previous and Next buttons.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-BINARY_TOC             = NO
-
-# The TOC_EXPAND flag can be set to YES to add extra items for group members to
-# the table of contents of the HTML help documentation and to the tree view.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-TOC_EXPAND             = NO
-
-# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
-# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
-# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
-# (.qch) of the generated HTML documentation.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_QHP           = NO
-
-# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
-# the file name of the resulting .qch file. The path specified is relative to
-# the HTML output folder.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QCH_FILE               =
-
-# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
-# Project output. For more information please see Qt Help Project / Namespace
-# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_NAMESPACE          = org.doxygen.Project
-
-# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
-# Help Project output. For more information please see Qt Help Project / Virtual
-# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
-# folders).
-# The default value is: doc.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_VIRTUAL_FOLDER     = doc
-
-# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
-# filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
-# filters).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_CUST_FILTER_NAME   =
-
-# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
-# custom filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
-# filters).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_CUST_FILTER_ATTRS  =
-
-# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
-# project's filter section matches. Qt Help Project / Filter Attributes (see:
-# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_SECT_FILTER_ATTRS  =
-
-# The QHG_LOCATION tag can be used to specify the location of Qt's
-# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
-# generated .qhp file.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHG_LOCATION           =
-
-# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
-# generated, together with the HTML files, they form an Eclipse help plugin. To
-# install this plugin and make it available under the help contents menu in
-# Eclipse, the contents of the directory containing the HTML and XML files needs
-# to be copied into the plugins directory of eclipse. The name of the directory
-# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
-# After copying Eclipse needs to be restarted before the help appears.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_ECLIPSEHELP   = NO
-
-# A unique identifier for the Eclipse help plugin. When installing the plugin
-# the directory name containing the HTML and XML files should also have this
-# name. Each documentation set should have its own identifier.
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
-
-ECLIPSE_DOC_ID         = org.doxygen.Project
-
-# If you want full control over the layout of the generated HTML pages it might
-# be necessary to disable the index and replace it with your own. The
-# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
-# of each HTML page. A value of NO enables the index and the value YES disables
-# it. Since the tabs in the index contain the same information as the navigation
-# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-DISABLE_INDEX          = NO
-
-# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
-# structure should be generated to display hierarchical information. If the tag
-# value is set to YES, a side panel will be generated containing a tree-like
-# index structure (just like the one that is generated for HTML Help). For this
-# to work a browser that supports JavaScript, DHTML, CSS and frames is required
-# (i.e. any modern browser). Windows users are probably better off using the
-# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
-# further fine-tune the look of the index. As an example, the default style
-# sheet generated by doxygen has an example that shows how to put an image at
-# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
-# the same information as the tab index, you could consider setting
-# DISABLE_INDEX to YES when enabling this option.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_TREEVIEW      = NO
-
-# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
-# doxygen will group on one line in the generated HTML documentation.
-#
-# Note that a value of 0 will completely suppress the enum values from appearing
-# in the overview section.
-# Minimum value: 0, maximum value: 20, default value: 4.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-ENUM_VALUES_PER_LINE   = 4
-
-# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
-# to set the initial width (in pixels) of the frame in which the tree is shown.
-# Minimum value: 0, maximum value: 1500, default value: 250.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-TREEVIEW_WIDTH         = 250
-
-# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
-# external symbols imported via tag files in a separate window.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-EXT_LINKS_IN_WINDOW    = NO
-
-# Use this tag to change the font size of LaTeX formulas included as images in
-# the HTML documentation. When you change the font size after a successful
-# doxygen run you need to manually remove any form_*.png images from the HTML
-# output directory to force them to be regenerated.
-# Minimum value: 8, maximum value: 50, default value: 10.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-FORMULA_FONTSIZE       = 10
-
-# Use the FORMULA_TRANPARENT tag to determine whether or not the images
-# generated for formulas are transparent PNGs. Transparent PNGs are not
-# supported properly for IE 6.0, but are supported on all modern browsers.
-#
-# Note that when changing this option you need to delete any form_*.png files in
-# the HTML output directory before the changes have effect.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-FORMULA_TRANSPARENT    = YES
-
-# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
-# http://www.mathjax.org) which uses client side Javascript for the rendering
-# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
-# installed or if you want to formulas look prettier in the HTML output. When
-# enabled you may also need to install MathJax separately and configure the path
-# to it using the MATHJAX_RELPATH option.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-USE_MATHJAX            = YES
-
-# When MathJax is enabled you can set the default output format to be used for
-# the MathJax output. See the MathJax site (see:
-# http://docs.mathjax.org/en/latest/output.html) for more details.
-# Possible values are: HTML-CSS (which is slower, but has the best
-# compatibility), NativeMML (i.e. MathML) and SVG.
-# The default value is: HTML-CSS.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_FORMAT         = HTML-CSS
-
-# When MathJax is enabled you need to specify the location relative to the HTML
-# output directory using the MATHJAX_RELPATH option. The destination directory
-# should contain the MathJax.js script. For instance, if the mathjax directory
-# is located at the same level as the HTML output directory, then
-# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
-# Content Delivery Network so you can quickly see the result without installing
-# MathJax. However, it is strongly recommended to install a local copy of
-# MathJax from http://www.mathjax.org before deployment.
-# The default value is: http://cdn.mathjax.org/mathjax/latest.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
-
-# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
-# extension names that should be enabled during MathJax rendering. For example
-# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_EXTENSIONS     =
-
-# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
-# of code that will be used on startup of the MathJax code. See the MathJax site
-# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
-# example see the documentation.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_CODEFILE       =
-
-# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
-# the HTML output. The underlying search engine uses javascript and DHTML and
-# should work on any modern browser. Note that when using HTML help
-# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
-# there is already a search function so this one should typically be disabled.
-# For large projects the javascript based search engine can be slow, then
-# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
-# search using the keyboard; to jump to the search box use <access key> + S
-# (what the <access key> is depends on the OS and browser, but it is typically
-# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
-# key> to jump into the search results window, the results can be navigated
-# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
-# the search. The filter options can be selected when the cursor is inside the
-# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
-# to select a filter and <Enter> or <escape> to activate or cancel the filter
-# option.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-SEARCHENGINE           = YES
-
-# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
-# implemented using a web server instead of a web client using Javascript. There
-# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
-# setting. When disabled, doxygen will generate a PHP script for searching and
-# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
-# and searching needs to be provided by external tools. See the section
-# "External Indexing and Searching" for details.
-# The default value is: NO.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SERVER_BASED_SEARCH    = NO
-
-# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
-# script for searching. Instead the search results are written to an XML file
-# which needs to be processed by an external indexer. Doxygen will invoke an
-# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
-# search results.
-#
-# Doxygen ships with an example indexer (doxyindexer) and search engine
-# (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: http://xapian.org/).
-#
-# See the section "External Indexing and Searching" for details.
-# The default value is: NO.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTERNAL_SEARCH        = NO
-
-# The SEARCHENGINE_URL should point to a search engine hosted by a web server
-# which will return the search results when EXTERNAL_SEARCH is enabled.
-#
-# Doxygen ships with an example indexer (doxyindexer) and search engine
-# (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: http://xapian.org/). See the section "External Indexing and
-# Searching" for details.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SEARCHENGINE_URL       =
-
-# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
-# search data is written to a file for indexing by an external tool. With the
-# SEARCHDATA_FILE tag the name of this file can be specified.
-# The default file is: searchdata.xml.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SEARCHDATA_FILE        = searchdata.xml
-
-# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
-# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
-# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
-# projects and redirect the results back to the right project.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTERNAL_SEARCH_ID     =
-
-# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
-# projects other than the one defined by this configuration file, but that are
-# all added to the same external search index. Each project needs to have a
-# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
-# to a relative location where the documentation can be found. The format is:
-# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTRA_SEARCH_MAPPINGS  =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the LaTeX output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
-# The default value is: YES.
-
-GENERATE_LATEX         = YES
-
-# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: latex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_OUTPUT           = latex
-
-# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
-# invoked.
-#
-# Note that when enabling USE_PDFLATEX this option is only used for generating
-# bitmaps for formulas in the HTML output, but not in the Makefile that is
-# written to the output directory.
-# The default file is: latex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_CMD_NAME         = latex
-
-# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
-# index for LaTeX.
-# The default file is: makeindex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-MAKEINDEX_CMD_NAME     = makeindex
-
-# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
-# documents. This may be useful for small projects and may help to save some
-# trees in general.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-COMPACT_LATEX          = NO
-
-# The PAPER_TYPE tag can be used to set the paper type that is used by the
-# printer.
-# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
-# 14 inches) and executive (7.25 x 10.5 inches).
-# The default value is: a4.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-PAPER_TYPE             = a4
-
-# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
-# that should be included in the LaTeX output. The package can be specified just
-# by its name or with the correct syntax as to be used with the LaTeX
-# \usepackage command. To get the times font for instance you can specify :
-# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
-# To use the option intlimits with the amsmath package you can specify:
-# EXTRA_PACKAGES=[intlimits]{amsmath}
-# If left blank no extra packages will be included.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-EXTRA_PACKAGES         =
-
-# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
-# generated LaTeX document. The header should contain everything until the first
-# chapter. If it is left blank doxygen will generate a standard header. See
-# section "Doxygen usage" for information on how to let doxygen write the
-# default header to a separate file.
-#
-# Note: Only use a user-defined header if you know what you are doing! The
-# following commands have a special meaning inside the header: $title,
-# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
-# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
-# string, for the replacement values of the other commands the user is referred
-# to HTML_HEADER.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_HEADER           =
-
-# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
-# generated LaTeX document. The footer should contain everything after the last
-# chapter. If it is left blank doxygen will generate a standard footer. See
-# LATEX_HEADER for more information on how to generate a default footer and what
-# special commands can be used inside the footer.
-#
-# Note: Only use a user-defined footer if you know what you are doing!
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_FOOTER           =
-
-# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
-# LaTeX style sheets that are included after the standard style sheets created
-# by doxygen. Using this option one can overrule certain style aspects. Doxygen
-# will copy the style sheet files to the output directory.
-# Note: The order of the extra style sheet files is of importance (e.g. the last
-# style sheet in the list overrules the setting of the previous ones in the
-# list).
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_EXTRA_STYLESHEET =
-
-# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
-# other source files which should be copied to the LATEX_OUTPUT output
-# directory. Note that the files will be copied as-is; there are no commands or
-# markers available.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_EXTRA_FILES      =
-
-# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
-# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
-# contain links (just like the HTML output) instead of page references. This
-# makes the output suitable for online browsing using a PDF viewer.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-PDF_HYPERLINKS         = YES
-
-# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
-# the PDF file directly from the LaTeX files. Set this option to YES, to get a
-# higher quality PDF documentation.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-USE_PDFLATEX           = YES
-
-# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
-# command to the generated LaTeX files. This will instruct LaTeX to keep running
-# if errors occur, instead of asking the user for help. This option is also used
-# when generating formulas in HTML.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_BATCHMODE        = NO
-
-# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
-# index chapters (such as File Index, Compound Index, etc.) in the output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_HIDE_INDICES     = NO
-
-# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
-# code with syntax highlighting in the LaTeX output.
-#
-# Note that which sources are shown also depends on other settings such as
-# SOURCE_BROWSER.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_SOURCE_CODE      = NO
-
-# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
-# bibliography, e.g. plainnat, or ieeetr. See
-# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
-# The default value is: plain.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_BIB_STYLE        = plain
-
-# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
-# page will contain the date and time when the page was generated. Setting this
-# to NO can help when comparing the output of multiple runs.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_TIMESTAMP        = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the RTF output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
-# RTF output is optimized for Word 97 and may not look too pretty with other RTF
-# readers/editors.
-# The default value is: NO.
-
-GENERATE_RTF           = NO
-
-# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: rtf.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_OUTPUT             = rtf
-
-# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
-# documents. This may be useful for small projects and may help to save some
-# trees in general.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-COMPACT_RTF            = NO
-
-# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
-# contain hyperlink fields. The RTF file will contain links (just like the HTML
-# output) instead of page references. This makes the output suitable for online
-# browsing using Word or some other Word compatible readers that support those
-# fields.
-#
-# Note: WordPad (write) and others do not support links.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_HYPERLINKS         = NO
-
-# Load stylesheet definitions from file. Syntax is similar to doxygen's config
-# file, i.e. a series of assignments. You only have to provide replacements,
-# missing definitions are set to their default value.
-#
-# See also section "Doxygen usage" for information on how to generate the
-# default style sheet that doxygen normally uses.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_STYLESHEET_FILE    =
-
-# Set optional variables used in the generation of an RTF document. Syntax is
-# similar to doxygen's config file. A template extensions file can be generated
-# using doxygen -e rtf extensionFile.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_EXTENSIONS_FILE    =
-
-# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
-# with syntax highlighting in the RTF output.
-#
-# Note that which sources are shown also depends on other settings such as
-# SOURCE_BROWSER.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_SOURCE_CODE        = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the man page output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
-# classes and files.
-# The default value is: NO.
-
-GENERATE_MAN           = NO
-
-# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it. A directory man3 will be created inside the directory specified by
-# MAN_OUTPUT.
-# The default directory is: man.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_OUTPUT             = man
-
-# The MAN_EXTENSION tag determines the extension that is added to the generated
-# man pages. In case the manual section does not start with a number, the number
-# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
-# optional.
-# The default value is: .3.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_EXTENSION          = .3
-
-# The MAN_SUBDIR tag determines the name of the directory created within
-# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
-# MAN_EXTENSION with the initial . removed.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_SUBDIR             =
-
-# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
-# will generate one additional man file for each entity documented in the real
-# man page(s). These additional files only source the real man page, but without
-# them the man command would be unable to find the correct page.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_LINKS              = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the XML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
-# captures the structure of the code including all documentation.
-# The default value is: NO.
-
-GENERATE_XML           = NO
-
-# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: xml.
-# This tag requires that the tag GENERATE_XML is set to YES.
-
-XML_OUTPUT             = xml
-
-# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
-# listings (including syntax highlighting and cross-referencing information) to
-# the XML output. Note that enabling this will significantly increase the size
-# of the XML output.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_XML is set to YES.
-
-XML_PROGRAMLISTING     = YES
-
-#---------------------------------------------------------------------------
-# Configuration options related to the DOCBOOK output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
-# that can be used to generate PDF.
-# The default value is: NO.
-
-GENERATE_DOCBOOK       = NO
-
-# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
-# front of it.
-# The default directory is: docbook.
-# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
-
-DOCBOOK_OUTPUT         = docbook
-
-# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
-# program listings (including syntax highlighting and cross-referencing
-# information) to the DOCBOOK output. Note that enabling this will significantly
-# increase the size of the DOCBOOK output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
-
-DOCBOOK_PROGRAMLISTING = NO
-
-#---------------------------------------------------------------------------
-# Configuration options for the AutoGen Definitions output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
-# AutoGen Definitions (see http://autogen.sf.net) file that captures the
-# structure of the code including all documentation. Note that this feature is
-# still experimental and incomplete at the moment.
-# The default value is: NO.
-
-GENERATE_AUTOGEN_DEF   = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the Perl module output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
-# file that captures the structure of the code including all documentation.
-#
-# Note that this feature is still experimental and incomplete at the moment.
-# The default value is: NO.
-
-GENERATE_PERLMOD       = NO
-
-# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
-# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
-# output from the Perl module output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_LATEX          = NO
-
-# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
-# formatted so it can be parsed by a human reader. This is useful if you want to
-# understand what is going on. On the other hand, if this tag is set to NO, the
-# size of the Perl module output will be much smaller and Perl will parse it
-# just the same.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_PRETTY         = YES
-
-# The names of the make variables in the generated doxyrules.make file are
-# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
-# so different doxyrules.make files included by the same Makefile don't
-# overwrite each other's variables.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_MAKEVAR_PREFIX =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the preprocessor
-#---------------------------------------------------------------------------
-
-# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
-# C-preprocessor directives found in the sources and include files.
-# The default value is: YES.
-
-ENABLE_PREPROCESSING   = YES
-
-# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
-# in the source code. If set to NO, only conditional compilation will be
-# performed. Macro expansion can be done in a controlled way by setting
-# EXPAND_ONLY_PREDEF to YES.
-# The default value is: NO.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-MACRO_EXPANSION        = NO
-
-# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
-# the macro expansion is limited to the macros specified with the PREDEFINED and
-# EXPAND_AS_DEFINED tags.
-# The default value is: NO.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-EXPAND_ONLY_PREDEF     = NO
-
-# If the SEARCH_INCLUDES tag is set to YES, the include files in the
-# INCLUDE_PATH will be searched if a #include is found.
-# The default value is: YES.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-SEARCH_INCLUDES        = YES
-
-# The INCLUDE_PATH tag can be used to specify one or more directories that
-# contain include files that are not input files but should be processed by the
-# preprocessor.
-# This tag requires that the tag SEARCH_INCLUDES is set to YES.
-
-INCLUDE_PATH           =
-
-# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
-# patterns (like *.h and *.hpp) to filter out the header-files in the
-# directories. If left blank, the patterns specified with FILE_PATTERNS will be
-# used.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-INCLUDE_FILE_PATTERNS  =
-
-# The PREDEFINED tag can be used to specify one or more macro names that are
-# defined before the preprocessor is started (similar to the -D option of e.g.
-# gcc). The argument of the tag is a list of macros of the form: name or
-# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
-# is assumed. To prevent a macro definition from being undefined via #undef or
-# recursively expanded use the := operator instead of the = operator.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-PREDEFINED             =
-
-# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
-# tag can be used to specify a list of macro names that should be expanded. The
-# macro definition that is found in the sources will be used. Use the PREDEFINED
-# tag if you want to use a different macro definition that overrules the
-# definition found in the source code.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-EXPAND_AS_DEFINED      =
-
-# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
-# remove all references to function-like macros that are alone on a line, have
-# an all uppercase name, and do not end with a semicolon. Such function macros
-# are typically used for boiler-plate code, and will confuse the parser if not
-# removed.
-# The default value is: YES.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-SKIP_FUNCTION_MACROS   = YES
-
-#---------------------------------------------------------------------------
-# Configuration options related to external references
-#---------------------------------------------------------------------------
-
-# The TAGFILES tag can be used to specify one or more tag files. For each tag
-# file the location of the external documentation should be added. The format of
-# a tag file without this location is as follows:
-# TAGFILES = file1 file2 ...
-# Adding location for the tag files is done as follows:
-# TAGFILES = file1=loc1 "file2 = loc2" ...
-# where loc1 and loc2 can be relative or absolute paths or URLs. See the
-# section "Linking to external documentation" for more information about the use
-# of tag files.
-# Note: Each tag file must have a unique name (where the name does NOT include
-# the path). If a tag file is not located in the directory in which doxygen is
-# run, you must also specify the path to the tagfile here.
-
-TAGFILES               =
-
-# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
-# tag file that is based on the input files it reads. See section "Linking to
-# external documentation" for more information about the usage of tag files.
-
-GENERATE_TAGFILE       =
-
-# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
-# the class index. If set to NO, only the inherited external classes will be
-# listed.
-# The default value is: NO.
-
-ALLEXTERNALS           = NO
-
-# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
-# in the modules index. If set to NO, only the current project's groups will be
-# listed.
-# The default value is: YES.
-
-EXTERNAL_GROUPS        = YES
-
-# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
-# the related pages index. If set to NO, only the current project's pages will
-# be listed.
-# The default value is: YES.
-
-EXTERNAL_PAGES         = YES
-
-# The PERL_PATH should be the absolute path and name of the perl script
-# interpreter (i.e. the result of 'which perl').
-# The default file (with absolute path) is: /usr/bin/perl.
-
-PERL_PATH              = /usr/bin/perl
-
-#---------------------------------------------------------------------------
-# Configuration options related to the dot tool
-#---------------------------------------------------------------------------
-
-# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
-# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
-# NO turns the diagrams off. Note that this option also works with HAVE_DOT
-# disabled, but it is recommended to install and use dot, since it yields more
-# powerful graphs.
-# The default value is: YES.
-
-CLASS_DIAGRAMS         = YES
-
-# You can define message sequence charts within doxygen comments using the \msc
-# command. Doxygen will then run the mscgen tool (see:
-# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
-# documentation. The MSCGEN_PATH tag allows you to specify the directory where
-# the mscgen tool resides. If left empty the tool is assumed to be found in the
-# default search path.
-
-MSCGEN_PATH            =
-
-# You can include diagrams made with dia in doxygen documentation. Doxygen will
-# then run dia to produce the diagram and insert it in the documentation. The
-# DIA_PATH tag allows you to specify the directory where the dia binary resides.
-# If left empty dia is assumed to be found in the default search path.
-
-DIA_PATH               =
-
-# If set to YES the inheritance and collaboration graphs will hide inheritance
-# and usage relations if the target is undocumented or is not a class.
-# The default value is: YES.
-
-HIDE_UNDOC_RELATIONS   = YES
-
-# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
-# available from the path. This tool is part of Graphviz (see:
-# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
-# Bell Labs. The other options in this section have no effect if this option is
-# set to NO
-# The default value is: YES.
-
-HAVE_DOT               = YES
-
-# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
-# to run in parallel. When set to 0 doxygen will base this on the number of
-# processors available in the system. You can set it explicitly to a value
-# larger than 0 to get control over the balance between CPU load and processing
-# speed.
-# Minimum value: 0, maximum value: 32, default value: 0.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_NUM_THREADS        = 0
-
-# When you want a differently looking font in the dot files that doxygen
-# generates you can specify the font name using DOT_FONTNAME. You need to make
-# sure dot is able to find the font, which can be done by putting it in a
-# standard location or by setting the DOTFONTPATH environment variable or by
-# setting DOT_FONTPATH to the directory containing the font.
-# The default value is: Helvetica.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTNAME           = Helvetica
-
-# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
-# dot graphs.
-# Minimum value: 4, maximum value: 24, default value: 10.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTSIZE           = 10
-
-# By default doxygen will tell dot to use the default font as specified with
-# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
-# the path where dot can find it using this tag.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTPATH           =
-
-# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
-# each documented class showing the direct and indirect inheritance relations.
-# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CLASS_GRAPH            = YES
-
-# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
-# graph for each documented class showing the direct and indirect implementation
-# dependencies (inheritance, containment, and class references variables) of the
-# class with other documented classes.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-COLLABORATION_GRAPH    = YES
-
-# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
-# groups, showing the direct groups dependencies.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GROUP_GRAPHS           = YES
-
-# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
-# collaboration diagrams in a style similar to the OMG's Unified Modeling
-# Language.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-UML_LOOK               = NO
-
-# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
-# class node. If there are many fields or methods and many nodes the graph may
-# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
-# number of items for each type to make the size more manageable. Set this to 0
-# for no limit. Note that the threshold may be exceeded by 50% before the limit
-# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
-# but if the number exceeds 15, the total amount of fields shown is limited to
-# 10.
-# Minimum value: 0, maximum value: 100, default value: 10.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-UML_LIMIT_NUM_FIELDS   = 10
-
-# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
-# collaboration graphs will show the relations between templates and their
-# instances.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-TEMPLATE_RELATIONS     = NO
-
-# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
-# YES then doxygen will generate a graph for each documented file showing the
-# direct and indirect include dependencies of the file with other documented
-# files.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INCLUDE_GRAPH          = YES
-
-# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
-# set to YES then doxygen will generate a graph for each documented file showing
-# the direct and indirect include dependencies of the file with other documented
-# files.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INCLUDED_BY_GRAPH      = YES
-
-# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
-# dependency graph for every global function or class method.
-#
-# Note that enabling this option will significantly increase the time of a run.
-# So in most cases it will be better to enable call graphs for selected
-# functions only using the \callgraph command. Disabling a call graph can be
-# accomplished by means of the command \hidecallgraph.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CALL_GRAPH             = YES
-
-# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
-# dependency graph for every global function or class method.
-#
-# Note that enabling this option will significantly increase the time of a run.
-# So in most cases it will be better to enable caller graphs for selected
-# functions only using the \callergraph command. Disabling a caller graph can be
-# accomplished by means of the command \hidecallergraph.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CALLER_GRAPH           = YES
-
-# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
-# hierarchy of all classes instead of a textual one.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GRAPHICAL_HIERARCHY    = YES
-
-# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
-# dependencies a directory has on other directories in a graphical way. The
-# dependency relations are determined by the #include relations between the
-# files in the directories.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DIRECTORY_GRAPH        = YES
-
-# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
-# generated by dot. For an explanation of the image formats see the section
-# output formats in the documentation of the dot tool (Graphviz (see:
-# http://www.graphviz.org/)).
-# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
-# to make the SVG files visible in IE 9+ (other browsers do not have this
-# requirement).
-# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
-# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
-# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
-# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
-# png:gdiplus:gdiplus.
-# The default value is: png.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_IMAGE_FORMAT       = png
-
-# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
-# enable generation of interactive SVG images that allow zooming and panning.
-#
-# Note that this requires a modern browser other than Internet Explorer. Tested
-# and working are Firefox, Chrome, Safari, and Opera.
-# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
-# the SVG files visible. Older versions of IE do not have SVG support.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INTERACTIVE_SVG        = NO
-
-# The DOT_PATH tag can be used to specify the path where the dot tool can be
-# found. If left blank, it is assumed the dot tool can be found in the path.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_PATH               =
-
-# The DOTFILE_DIRS tag can be used to specify one or more directories that
-# contain dot files that are included in the documentation (see the \dotfile
-# command).
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOTFILE_DIRS           =
-
-# The MSCFILE_DIRS tag can be used to specify one or more directories that
-# contain msc files that are included in the documentation (see the \mscfile
-# command).
-
-MSCFILE_DIRS           =
-
-# The DIAFILE_DIRS tag can be used to specify one or more directories that
-# contain dia files that are included in the documentation (see the \diafile
-# command).
-
-DIAFILE_DIRS           =
-
-# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
-# path where java can find the plantuml.jar file. If left blank, it is assumed
-# PlantUML is not used or called during a preprocessing step. Doxygen will
-# generate a warning when it encounters a \startuml command in this case and
-# will not generate output for the diagram.
-
-PLANTUML_JAR_PATH      =
-
-# When using plantuml, the specified paths are searched for files specified by
-# the !include statement in a plantuml block.
-
-PLANTUML_INCLUDE_PATH  =
-
-# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
-# that will be shown in the graph. If the number of nodes in a graph becomes
-# larger than this value, doxygen will truncate the graph, which is visualized
-# by representing a node as a red box. Note that doxygen if the number of direct
-# children of the root node in a graph is already larger than
-# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
-# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
-# Minimum value: 0, maximum value: 10000, default value: 50.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_GRAPH_MAX_NODES    = 50
-
-# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
-# generated by dot. A depth value of 3 means that only nodes reachable from the
-# root by following a path via at most 3 edges will be shown. Nodes that lay
-# further from the root node will be omitted. Note that setting this option to 1
-# or 2 may greatly reduce the computation time needed for large code bases. Also
-# note that the size of a graph can be further restricted by
-# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
-# Minimum value: 0, maximum value: 1000, default value: 0.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-MAX_DOT_GRAPH_DEPTH    = 0
-
-# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
-# background. This is disabled by default, because dot on Windows does not seem
-# to support this out of the box.
-#
-# Warning: Depending on the platform used, enabling this option may lead to
-# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
-# read).
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_TRANSPARENT        = NO
-
-# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
-# files in one run (i.e. multiple -o and -T options on the command line). This
-# makes dot run faster, but since only newer versions of dot (>1.8.10) support
-# this, this feature is disabled by default.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_MULTI_TARGETS      = NO
-
-# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
-# explaining the meaning of the various boxes and arrows in the dot generated
-# graphs.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GENERATE_LEGEND        = YES
-
-# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
-# files that are used to generate the various graphs.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_CLEANUP            = YES
diff --git a/fixposition_driver_lib/README.md b/fixposition_driver_lib/README.md
index 354b9bc..959ae55 100644
--- a/fixposition_driver_lib/README.md
+++ b/fixposition_driver_lib/README.md
@@ -1,37 +1,5 @@
 # Fixposition Driver Lib
 
-This is a CMake library used to parse [Fixposition ASCII messages](https://docs.fixposition.com/fd/i-o-messages). The message content will be converted into a generic struct and can be processed further from there.
+[ROS](https://www.ros.org/) messages for [Fixposition Vision-RTK 2](https://www.fixposition.com/product).
 
-It can be built using plain CMake, or using catkin or colcon depending on which ROS version is used.
-
-## Dependencies
--  [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page), tested with version [3.3.7](https://gitlab.com/libeigen/eigen/-/releases/3.3.7)
--  [Boost](https://www.boost.org/), tested with version [1.65.0](https://www.boost.org/users/history/version_1_65_0.html)
--  [CMake](https://cmake.org/)
-
-ROS1:
--  [Catkin](http://wiki.ros.org/catkin)
-
-ROS2
--  [Colcon](https://colcon.readthedocs.io/en/released/)
-
-
-## Build
-### CMake
-from the repository root dir:
-```
-cd fixposition_driver_lib
-mkdir build
-cd build
-cmake ..
-make
-```
-
-### Catkin
-clone this repository under the `src` folder in the catkin workspace, and then run `catkin build fixposition_driver_lib` or if you want to build all packages, run `catkin build`.
-
-### Colcon
-clone this repository under the `src` folder in the colcon workspace, and then run `colcon build --packages-up-to fixposition_driver_lib` or if you want to build all packages, run `colcon build`
-
-### Use this library in other projects:
-A cmake file is provided in the cmake directory, please see [fixposition_driver_ros1](../fixposition_driver_ros1) or [fixposition_driver_ros2](../fixposition_driver_ros2) as examples of using this library.
+See the [main README.md](../README.md) for the documentation.
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp
index 9d1bc80..f3142e8 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp
@@ -1,156 +1,239 @@
 /**
- *  @file
- *  @brief Declaration of FixpositionDriver class
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Fixposition sensor driver
  */
 
-#ifndef __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__
-#define __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__
+#ifndef __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER_HPP__
+#define __FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER_HPP__
 
-/* SYSTEM / STL */
-#include <termios.h>
+/* LIBC/STL */
+#include <array>
+#include <cstdint>
+#include <functional>
+#include <string>
+#include <unordered_map>
+#include <vector>
 
 /* EXTERNAL */
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/fpb_measurements.hpp>
-#include <fixposition_driver_lib/helper.hpp>
-#include <fixposition_driver_lib/params.hpp>
+#include <poll.h>
+#include <termios.h>
+
+#include <boost/core/noncopyable.hpp>
+#include <fpsdk_common/parser.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/fpb.hpp>
+#include <fpsdk_common/parser/nmea.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_common/thread.hpp>
+
+/* PACKAGE */
+#include "fixposition_driver_lib/helper.hpp"
+#include "fixposition_driver_lib/params.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-class FixpositionDriver {
+/**
+ * @brief Fixposition driver
+ *
+ * This handles the connection to a Fixposition Vision-RTK 2 sensor. It receives and decodes the received messages,
+ * which can be used by consumers by observing ("subscribing to") the driver. On various events, such as receiving
+ * a new message or completing and "epoch", the observers are notified by the driver. The driver also handles sending
+ * data to the sensor, namely GNSS correction data (typically, RTCM3 messages) or measurements, such as wheelspeed.
+ */
+class FixpositionDriver : private boost::noncopyable {
    public:
     /**
-     * @brief Construct a new FixpositionDriver object
+     * @brief Constructor
      *
+     * @params[in]  params  Parameters
      */
-    FixpositionDriver(const FixpositionDriverParams& params);
+    FixpositionDriver(const DriverParams& params);
 
     /**
-     * @brief Destroy the Fixposition Driver object, close all open connections
-     *
+     * @brief Destructor
      */
-    ~FixpositionDriver();
+    virtual ~FixpositionDriver();
+
+    //
+    // ----- Methods for starting and stopping the driver -----
 
     /**
-     * @brief Run in Loop the Read Convert and Publish cycle
+     * @brief Connect to sensor and start the worker thread
      *
+     * @returns true on success, false otherwise
      */
-    virtual bool RunOnce();
+    bool StartDriver();
 
-   protected:
     /**
-     * @brief
-     *
-     * @param[in] sensors_meas map wheelspeed of sensors, each sensor containing speed values and their validity flag
+     * @brief Stop the worker thread and disconnect from sensor
      */
-    virtual void WsCallback(const std::unordered_map<std::string, std::vector<std::pair<bool, int>>>& sensors_meas);
+    void StopDriver();
+
+    //
+    // ----- Methods for observing messages received from the sensor -----
+
+    //! Observer function for FP_A messages
+    using FpaObserver = std::function<void(const fpsdk::common::parser::fpa::FpaPayload&)>;
+
+    //! Observer function for NMEA messages
+    using NmeaObserver = std::function<void(const fpsdk::common::parser::nmea::NmeaPayload&)>;
+
+    //! Observer function for NOV_B messages
+    using NovbObserver = std::function<void(const fpsdk::common::parser::novb::NovbHeader*, const uint8_t*)>;
+
+    //! Observer function for raw parser messages
+    using RawObserver = std::function<void(const fpsdk::common::parser::ParserMsg&)>;
 
     /**
-     * @brief
+     * @brief Add observer for FP_A message
      *
-     * @param[in] rtcm_msg string with ASCII data for RTK correction
+     * @param[in]  message_name   The message name to observe, e.g. "FP_A-ODOMETRY"
+     * @param[in]  observer       The function to call to process the message
      */
-    virtual void RtcmCallback(const void *rtcm_msg, const size_t msg_size);
+    void AddFpaObserver(const std::string& message_name, FpaObserver observer);
 
     /**
-     * @brief
+     * @brief Add observer for NMEA message
      *
-     * @param[in] meas_vec measurements from one specific wheelspeed sensor, with their validity flag
-     * @param[in] meas_loc location from the specific wheelspeed sensor
-     * @param[out] meas_fpb fpb measurement to be filled from the vector
-     * @return true if the measurement was successfully filled, false otherwise
+     * @param[in]  formatter  The formatter for the messages to observe, e.g. "GGA"
+     * @param[in]  observer   The function to call to process the message
      */
-    virtual bool FillWsSensorMeas(const std::vector<std::pair<bool, int>>& meas_vec,
-                                  const FpbMeasurementsMeasLoc meas_loc, FpbMeasurementsMeas& meas_fpb);
+    void AddNmeaObserver(const std::string& formatter, NmeaObserver observer);
 
     /**
-     * @brief Converts the measurement location from string to the enum values
+     * @brief Add observer for NOV_B message
      *
-     * @param[in] meas_loc user input location in string format
-     * @return FpbMeasurementsMeasLoc converted measurement location
+     * @param[in]  message_name   The message name to observe, e.g. "NOV_B-BESTGNSSPOS"
+     * @param[in]  observer       The function to call to process the message
      */
-    virtual FpbMeasurementsMeasLoc WsMeasStringToLoc(const std::string& meas_loc);
+    void AddNovbObserver(const std::string& message_name, NovbObserver observer);
 
     /**
-     * @brief Convert the Nmea like string using correct converter
+     * @brief Add observer for raw parser messages
      *
-     * @param[in] msg NMEA like string to be converted. $HEADER,,,,,,,*CHECKSUM
+     * @param[in]  observer  The function to call to process the message
      */
-    virtual void NmeaConvertAndPublish(const std::string& msg);
+    void AddRawObserver(RawObserver observer);
 
     /**
-     * @brief Convert the buffer after identified as Nov msg
-     *
-     * @param[in] msg ptr to the start of the msg
+     * @brief Remove all observers for FP_A messages
+     */
+    void RemoveFpaObservers();
+
+    /**
+     * @brief Remove all observers for NMEA messages
      */
-    virtual void NovConvertAndPublish(const uint8_t* msg);
+    void RemoveNmeaObservers();
 
     /**
-     * @brief Initialize convertes based on config
+     * @brief Remove all observers for NOV_B messages
+     */
+    void RemoveNovbObservers();
+
+    /**
+     * @brief Remove all observers for raw parser messages
+     */
+    void RemoveRawObservers();
+
+    /**
+     * @brief Send correction data to sensor
      *
-     * @return true
-     * @return false
+     * @param[in]  msg   The raw message data (e.g. a RTCM3 message)
+     * @param[in]  size  Size of the raw message
      */
-    virtual bool InitializeConverters();
+    void SendCorrectionData(const uint8_t* msg, const std::size_t size);
 
     /**
-     * @brief Read data and publish to ros if possible
+     * @brief Send wheelspeed data to sensor
      *
-     * @return true data read success or no data
-     * @return false connection problems, restart the connection
+     * @param[in]  data  The wheelspeed data
      */
-    virtual bool ReadAndPublish();
+    void SendWheelspeedData(const std::vector<WheelSpeedData>& data);
+
+   protected:
+    DriverParams params_;
+
+    // ---- Methods to handle the sensor connection ----
 
     /**
-     * @brief Connect the defined TCP or Serial socket
+     * @brief Connect to sensor
      *
-     * @return true success
-     * @return false cannot connect
+     * @returns true on success, false otherwise
      */
     virtual bool Connect();
 
     /**
-     * @brief Initialize TCP connection
+     * @brief Disconnect from sensor
      *
-     * @return true success
-     * @return false fail
+     * @returns true on success, false otherwise
      */
-    virtual bool CreateTCPSocket();
+    virtual void Disconnect();
 
     /**
-     * @brief Initialize Serial connection
+     * @brief Check connection
      *
-     * @return true success
-     * @return false fail
+     * @returns true if connection is up, false otherwise
      */
-    virtual bool CreateSerialConnection();
-
-    FixpositionDriverParams params_;
-
-    std::unordered_map<std::string, std::unique_ptr<BaseAsciiConverter>>
-        a_converters_;  //!< ascii converters corresponding to the input formats
+    virtual bool IsConnected() const;
 
-    using BestgnssposObserver = std::function<void(const Oem7MessageHeaderMem*, const BESTGNSSPOSMem*)>;
-    std::vector<BestgnssposObserver> bestgnsspos_obs_;  //!< observers for bestgnsspos
-
-    // TODO: Add more NOV types
+    /**
+     * @brief Read data
+     *
+     * @param[out]  buf      Buffer to store read data
+     * @param[in]   size     Size of buffer
+     * @param[in]   timeout  Timeout [ms] to wait for data
+     *
+     * @returns the size read
+     */
+    virtual std::size_t Read(uint8_t* buf, const std::size_t size, const int timeout);
 
-    int client_fd_ = -1;  //!< TCP or Serial file descriptor
-    int connection_status_ = -1;
-    struct termios options_save_;
-    uint8_t readBuf[8192];
-    int buf_size = 0;
+    /**
+     * @brief Write data
+     *
+     * @param[in]  bu f  Buffer to the data
+     * @param[in]  size  Size of the data
+     *
+     * @returns true if the data was sent, false otherwise
+     */
+    virtual bool Write(const uint8_t* buf, const std::size_t size);
+
+   private:
+    // Sensor connection
+    bool ConnectTcp(const std::string& ip, const int port);      //!< Connect() for TCP
+    void DisconnectTcp();                                        //!< Disconnect() for TCP
+    bool ConnectSerial(const std::string& dev, const int baud);  //!< Connect() for serial
+    void DisconnectSerial();                                     //!< Disconnect() for serial
+    bool serial_ = false;                                        //!< Serial (true) or TCP client (false)
+    int sensor_fd_ = -1;                                         //!< TCP or serial file descriptor
+    std::string sensor_name_;                                    //!< Name of the sensor, for debugging
+    std::array<struct pollfd, 1> poll_fds_;                      //!< poll() config
+    struct termios options_save_;                                //!< Saved serial port parameters
+
+    // Worker thread
+    fpsdk::common::parser::Parser parser_;  //!< Protocol parser for incoming messages
+    fpsdk::common::thread::Thread worker_;  //!< Worker thread handle
+    void Worker(void* arg);                 //!< Worker thread
+
+    // Observers for received messages
+    std::unordered_map<std::string, std::vector<FpaObserver>> fpa_observers_;    //!< FP_A message observers
+    std::unordered_map<std::string, std::vector<NmeaObserver>> nmea_observers_;  //!< NMEA message observers
+    std::unordered_map<std::string, std::vector<NovbObserver>> novb_observers_;  //!< NOV_B message observers
+    std::vector<RawObserver> raw_observers_;                                     //!< Raw parser message observers
+    void NotifyFpaObservers(const fpsdk::common::parser::ParserMsg& msg);        //!< Notify FP_A message observers
+    void NotifyNmeaObservers(const fpsdk::common::parser::ParserMsg& msg);       //!< Notify NMEA message observers
+    void NotifyNovbObservers(const fpsdk::common::parser::ParserMsg& msg);       //!< Notify NOV_B message observers
+    void NotifyRawObservers(const fpsdk::common::parser::ParserMsg& msg);        //!< Notify raw message observers
 };
+
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-#endif  //__FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER__
+#endif  //__FIXPOSITION_DRIVER_LIB_FIXPOSITION_DRIVER_HPP__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp
deleted file mode 100644
index 4a7fe13..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp
+++ /dev/null
@@ -1,164 +0,0 @@
-/**
- *  @file
- *  @brief Helper functions
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_GNSS_TF__
-#define __FIXPOSITION_DRIVER_LIB_GNSS_TF__
-
-/* PACKAGE */
-#include <fixposition_driver_lib/time_conversions.hpp>
-
-namespace fixposition {
-
-/**
- * @brief Calculate the rotation matrix from ECEF to
- * ENU for a given reference latitude/longitude.
- *
- * @param[in] lat Reference latitude [rad]
- * @param[in] lon Reference longitude [rad]
- * @return Eigen::Matrix3d Rotation matrix from ECEF -> ENU
- */
-Eigen::Matrix3d RotEnuEcef(double lat, double lon);
-
-/**
- * @brief Calculate the rotation matrix from ECEF to
- * ENU for a given reference origin.
- *
- * @param[in] in_pos Reference position in ECEF [m]
- * @return Eigen::Matrix3d Rotation matrix ECEF -> ENU
- */
-Eigen::Matrix3d RotEnuEcef(const Eigen::Vector3d &ecef);
-
-/**
- * @brief Returns rotation matrix between NED and ENU
- * @details | 0, 1, 0 |
- *          | 1, 0, 0 |
- *          | 0, 0,-1 |
- *
- * @return Eigen::Matrix3d
- */
-Eigen::Matrix3d RotNedEnu();
-
-/**
- * @brief Calculate the rotation matrix from ECEF to
- * NED for a given reference latitude/longitude.
- *
- * @param[in] lat Reference latitude [rad]
- * @param[in] lon Reference longitude [rad]
- * @return Eigen::Matrix3d Rotation matrix from ECEF -> NED
- */
-Eigen::Matrix3d RotNedEcef(double lat, double lon);
-
-/**
- * @brief Calculate the rotation matrix from ECEF to
- * NED for a given reference origin.
- *
- * @param[in] in_pos Reference position in ECEF [m]
- * @return Eigen::Matrix3d Rotation matrix ECEF -> NED
- */
-Eigen::Matrix3d RotNedEcef(const Eigen::Vector3d &ecef);
-
-/**
- * @brief Transform ECEF coordinate to ENU with specified ENU-origin
- *
- * @param[in] xyz ECEF position to be transsformed [m]
- * @param[in] wgs84llh_ref ENU-origin in Geodetic coordinates (Lat[rad], Lon[rad], Height[m])
- * @return Eigen::Vector3d Position in ENU coordinates
- */
-Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref);
-
-/**
- * @brief  Transform ENU coordinate to ECEF with specified ENU-origin
- *
- * @param[in] enu ENU position to be transsformed [m]
- * @param[in] wgs84llh_ref ENU-origin in Geodetic coordinates (Lat[rad], Lon[rad], Height[m])
- * @return Eigen::Vector3d
- */
-Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d &enu, const Eigen::Vector3d &wgs84llh_ref);
-
-Eigen::Vector3d TfNedEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref);
-Eigen::Vector3d TfEcefNed(const Eigen::Vector3d &ned, const Eigen::Vector3d &wgs84llh_ref);
-
-/**
- * @brief Convert Geodetic coordinates (latitude, longitude, height) to
- * ECEF (x, y, z).
- *
- * @param[in] wgs84llh Geodetic coordinates (Lat[rad], Lon[rad], Height[m])
- * @return Eigen::Vector3d ECEF coordinates [m]
- */
-Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d &wgs84llh);
-
-/**
- * @brief Convert ECEF (x, y, z) coordinates to Lat Lon Height coordinates
- * (latitude, longitude, altitude).
- *
- * @param[in] ecef ECEF coordinates [m]
- * @return Eigen::Vector3d Geodetic coordinates (Lat[rad], Lon[rad], Height[m])
- */
-Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d &ecef);
-
-/**
- * @brief Given Pose in ECEF, calculate Yaw-Pitch-Roll in ENU
- * @details Yaw will be -Pi/2 when X is pointing North, because ENU starts with East
- *
- * @param ecef_p 3D Position Vector in ECEF
- * @param ecef_r 3x3 Rotation matrix representing rotation Body --> ECEF
- * @return Eigen::Vector3d Yaw-Pitch-Roll in ENU
- * (Yaw will be -Pi/2 when X is pointing North, because ENU starts with
-
- */
-Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r);
-
-/**
- * @brief
- *
- * @param ecef_p 3D Position Vector in ECEF
- * @param ecef_r 3x3 Rotation matrix representing rotation Body --> ECEF
- * @return Eigen::Vector3d Yaw-Pitch-Roll in NED
- */
-Eigen::Vector3d EcefPoseToNedEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r);
-
-/**
- * @brief Vector4 quaternion to intrinsic Euler Angles in ZYX (yaw,pitch,roll)
- *
- * @param[in] quat Eigen::Quaterniond in w,i,j,k
- * @return Eigen::Vector3d intrinsic euler angle in ZYX (Yaw/Pitch/Roll) order
- */
-inline Eigen::Vector3d QuatToEul(const Eigen::Quaterniond &q) {
-    auto qw = q.w();
-    auto qx = q.x();
-    auto qy = q.y();
-    auto qz = q.z();
-    auto eul0 = atan2(2 * (qx * qy + qw * qz), qw * qw + qx * qx - qy * qy - qz * qz);
-    auto eul1 = asin(-2.0 * (qx * qz - qw * qy));
-    auto eul2 = atan2(2 * (qy * qz + qw * qx), qw * qw - qx * qx - qy * qy + qz * qz);
-
-    Eigen::Vector3d eul;
-    eul << eul0, eul1, eul2;
-
-    return eul;
-}
-
-/**
- * @brief Rotation Matrix to intrinsic Euler Angles in ZYX (Yaw-Pitch-Roll) order in radian
- *
- * @param rot Eigen::Matrix3d
- * @return Eigen::Vector3d intrinsic euler angle in ZYX (Yaw-Pitch-Roll) order in radian
- */
-inline Eigen::Vector3d RotToEul(const Eigen::Matrix3d &rot) {
-    const Eigen::Quaterniond quat(rot);
-    return QuatToEul(quat);
-}
-
-}  // namespace fixposition
-#endif  // __FIXPOSITION_DRIVER_LIB_GNSS_TF__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp
index 839673e..8ea1930 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/helper.hpp
@@ -1,62 +1,263 @@
 /**
- *  @file
- *  @brief Helper functions
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Helper functions and types
  */
 
-#ifndef __FIXPOSITION_DRIVER_LIB_HELPER__
-#define __FIXPOSITION_DRIVER_LIB_HELPER__
+#ifndef __FIXPOSITION_DRIVER_LIB_HELPER_HPP__
+#define __FIXPOSITION_DRIVER_LIB_HELPER_HPP__
 
-/* SYSTEM / STL */
+/* LIBC/STL */
+#include <cstdint>
 #include <string>
 #include <vector>
 
 /* EXTERNAL */
-#include <boost/algorithm/string.hpp>
-#include <boost/algorithm/string/split.hpp>
-#include <fixposition_driver_lib/messages/msg_data.hpp>
-#include <fixposition_driver_lib/messages/nov_type.hpp>
+#include <fpsdk_common/ext/eigen_core.hpp>
+#include <fpsdk_common/ext/eigen_geometry.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/fpb.hpp>
+#include <fpsdk_common/parser/nmea.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_common/time.hpp>
+
+/* PACKAGE */
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-/**
- * @brief
- *
- * @param[in] msg
- * @param[in] delim
- * @param[out] tokens
- */
-void SplitMessage(std::vector<std::string>& tokens, const std::string& msg, const std::string& delim);
+static constexpr const char* ODOMETRY_FRAME_ID = "FP_ECEF";
+static constexpr const char* ODOMETRY_CHILD_FRAME_ID = "FP_POI";
+
+static constexpr const char* ODOMENU_FRAME_ID = "FP_ENU0";
+static constexpr const char* ODOMENU_CHILD_FRAME_ID = "FP_POI";
+
+static constexpr const char* ODOMSH_FRAME_ID = "FP_ECEF";
+static constexpr const char* ODOMSH_CHILD_FRAME_ID = "FP_POISH";
+
+static constexpr const char* IMU_FRAME_ID = "FP_VRTK";
+
+static constexpr const char* GNSS_FRAME_ID = "GNSS";
+static constexpr const char* GNSS1_FRAME_ID = "GNSS1";
+static constexpr const char* GNSS2_FRAME_ID = "GNSS2";
+
+static constexpr const char* ENU_FRAME_ID = "FP_ENU";
+
+fpsdk::common::time::Time FpaGpsTimeToTime(const fpsdk::common::parser::fpa::FpaGpsTime gps_time);
+
+struct TfData {
+    bool valid = false;
+    fpsdk::common::time::Time stamp;
+    std::string frame_id;
+    std::string child_frame_id;
+    Eigen::Vector3d translation;
+    Eigen::Quaterniond rotation;
+    TfData() {
+        translation.setZero();
+        rotation.setIdentity();
+    }
+    bool SetFromFpaTfPayload(const fpsdk::common::parser::fpa::FpaTfPayload& payload);
+};
+
+struct PoseWithCovData {
+    bool valid = false;
+    Eigen::Vector3d position;
+    Eigen::Quaterniond orientation;
+    Eigen::Matrix<double, 6, 6> cov;
+    PoseWithCovData() {
+        position.setZero();
+        orientation.setIdentity();
+        cov.setZero();
+    }
+    bool SetFromFpaOdomPayload(const fpsdk::common::parser::fpa::FpaOdomPayload& payload);
+};
+
+struct TwistWithCovData {
+    bool valid = false;
+    Eigen::Vector3d linear;
+    Eigen::Vector3d angular;
+    Eigen::Matrix<double, 6, 6> cov;
+    TwistWithCovData() {
+        linear.setZero();
+        angular.setZero();
+        cov.setZero();
+    }
+    bool SetFromFpaOdomPayload(const fpsdk::common::parser::fpa::FpaOdomPayload& payload);
+};
+
+struct OdometryData {
+    bool valid = false;
+    enum class Type { UNSPECIFIED, ODOMETRY, ODOMENU, ODOMSH };
+    Type type = Type::UNSPECIFIED;
+    fpsdk::common::time::Time stamp;
+    std::string frame_id;
+    std::string child_frame_id;
+    PoseWithCovData pose;
+    TwistWithCovData twist;
+    bool SetFromFpaOdomPayload(const fpsdk::common::parser::fpa::FpaOdomPayload& payload);
+};
 
 /**
- * @brief
+ * @brief Build a 3x3 covariance matrix
  *
- * @param[in] header
- * @param[in] bestgnsspos
- * @param[out] navsatfix
+ *    | xx xy xz |
+ *    | xy yy yz |
+ *    | xz yz zz |
  */
-void BestGnssPosToNavSatFix(const Oem7MessageHeaderMem* const header, const BESTGNSSPOSMem* const bestgnsspos,
-                            NavSatFixData& navsatfix);
+inline Eigen::Matrix<double, 3, 3> BuildCovMat3D(const double xx, const double yy, const double zz, const double xy,
+                                                 const double yz, const double xz) {
+    Eigen::Matrix<double, 3, 3> cov;
+    cov.setZero();
+
+    // Diagonals
+    cov(0, 0) = xx;  // 0
+    cov(1, 1) = yy;  // 4
+    cov(2, 2) = zz;  // 8
+
+    // Rest of values
+    cov(1, 0) = cov(0, 1) = xy;  // 1 = 3
+    cov(2, 1) = cov(1, 2) = yz;  // 2 = 6
+    cov(2, 0) = cov(0, 2) = xz;  // 5 = 7
+
+    return cov;
+}
 
 /**
- * @brief Convert NOV to other data structs
+ * @brief Build a 6x6 covariance matrix which is 2 independent 3x3 matrices
  *
- * @tparam T_nov NOV struct type
- * @tparam T_data Data struct type
- * @param[in] header
- * @param[in] nov
- * @param[out] data
+ *    | xx  xy  xz   0    0    0  |
+ *    | xy  yy  yz   0    0    0  |
+ *    | xz  yz  zz   0    0    0  |
+ *    |  0   0   0  xx1  xy1  xz1 |
+ *    |  0   0   0  xy1  yy1  yz1 |
+ *    |  0   0   0  xz1  yz1  zz1 |
  */
-template <class T_nov, class T_data>
-void NovToData(const Oem7MessageHeaderMem* const header, const T_nov* const nov, T_data& data);
+inline Eigen::Matrix<double, 6, 6> BuildCovMat6D(const double xx, const double yy, const double zz, const double xy,
+                                                 const double yz, const double xz, double xx1, const double yy1,
+                                                 const double zz1, const double xy1, const double yz1, double xz1) {
+    Eigen::Matrix<double, 6, 6> cov;
+    cov.setZero();
+
+    // Diagonals
+    cov(0, 0) = xx;   // 0
+    cov(1, 1) = yy;   // 7
+    cov(2, 2) = zz;   // 14
+    cov(3, 3) = xx1;  // 21
+    cov(4, 4) = yy1;  // 28
+    cov(5, 5) = zz1;  // 35
+
+    // Rest of values
+    cov(1, 0) = cov(0, 1) = xy;   // 1 = 6
+    cov(2, 1) = cov(1, 2) = yz;   // 8 = 13
+    cov(2, 0) = cov(0, 2) = xz;   // 2 = 12
+    cov(4, 3) = cov(3, 4) = xy1;  // 22 = 27
+    cov(5, 4) = cov(4, 5) = yz1;  // 29 = 34
+    cov(5, 3) = cov(3, 5) = xz1;  // 23 = 33
+
+    return cov;
+}
+
+struct JumpDetector {
+    JumpDetector();
+    bool Check(const OdometryData& odometry_data);
+
+    Eigen::Vector3d curr_pos_;
+    Eigen::MatrixXd curr_cov_;
+    fpsdk::common::time::Time curr_stamp_;
+    Eigen::Vector3d prev_pos_;
+    Eigen::MatrixXd prev_cov_;
+    fpsdk::common::time::Time prev_stamp_;
+    Eigen::Vector3d pos_diff_;
+    std::string warning_;
+};
+
+struct WheelSpeedData {
+    std::string location_;
+    bool vx_valid_ = false;
+    int32_t vx_ = 0;
+    bool vy_valid_ = false;
+    int32_t vy_ = 0;
+    bool vz_valid_ = false;
+    int32_t vz_ = 0;
+};
+
+template <typename RosMsgT>  // e.g. fixposition_driver_msgs::Speed
+inline std::vector<WheelSpeedData> SpeedMsgToWheelspeedData(const RosMsgT& msg) {
+    std::vector<WheelSpeedData> data;
+    for (const auto& sensor : msg.sensors) {
+        data.push_back(
+            {sensor.location, sensor.vx_valid, sensor.vx, sensor.vy_valid, sensor.vy, sensor.vz_valid, sensor.vz});
+    }
+    return data;
+}
+
+struct NmeaEpochData {
+    NmeaEpochData(const fpsdk::common::parser::fpa::FpaEpoch epoch);
+
+    // Collectors
+    fpsdk::common::parser::fpa::FpaEpoch epoch_;
+    fpsdk::common::parser::nmea::NmeaGgaPayload gga_;
+    fpsdk::common::parser::nmea::NmeaGllPayload gll_;
+    fpsdk::common::parser::nmea::NmeaGsaPayload gsa_;
+    fpsdk::common::parser::nmea::NmeaGstPayload gst_;
+    fpsdk::common::parser::nmea::NmeaHdtPayload hdt_;
+    fpsdk::common::parser::nmea::NmeaRmcPayload rmc_;
+    fpsdk::common::parser::nmea::NmeaVtgPayload vtg_;
+    fpsdk::common::parser::nmea::NmeaZdaPayload zda_;
+    fpsdk::common::parser::nmea::NmeaCollectGsaGsv gsa_gsv_;
+
+    // "Best of" data, populated by Complete()
+    fpsdk::common::time::Time stamp_;
+    std::string frame_id_;
+    fpsdk::common::parser::nmea::NmeaDate date_;
+    fpsdk::common::parser::nmea::NmeaTime time_;
+    fpsdk::common::parser::nmea::NmeaStatusGllRmc status_ = fpsdk::common::parser::nmea::NmeaStatusGllRmc::UNSPECIFIED;
+    fpsdk::common::parser::nmea::NmeaNavStatusRmc navstatus_ =
+        fpsdk::common::parser::nmea::NmeaNavStatusRmc::UNSPECIFIED;
+    fpsdk::common::parser::nmea::NmeaModeRmcGns mode1_ = fpsdk::common::parser::nmea::NmeaModeRmcGns::UNSPECIFIED;
+    fpsdk::common::parser::nmea::NmeaModeGllVtg mode2_ = fpsdk::common::parser::nmea::NmeaModeGllVtg::UNSPECIFIED;
+    fpsdk::common::parser::nmea::NmeaQualityGga quality_ = fpsdk::common::parser::nmea::NmeaQualityGga::UNSPECIFIED;
+    fpsdk::common::parser::nmea::NmeaOpModeGsa opmode_ = fpsdk::common::parser::nmea::NmeaOpModeGsa::UNSPECIFIED;
+    fpsdk::common::parser::nmea::NmeaNavModeGsa navmode_ = fpsdk::common::parser::nmea::NmeaNavModeGsa::UNSPECIFIED;
+    fpsdk::common::parser::nmea::NmeaLlh llh_;
+    fpsdk::common::parser::nmea::NmeaInt num_sv_;
+    fpsdk::common::parser::nmea::NmeaFloat rms_range_;
+    fpsdk::common::parser::nmea::NmeaFloat std_major_;
+    fpsdk::common::parser::nmea::NmeaFloat std_minor_;
+    fpsdk::common::parser::nmea::NmeaFloat angle_major_;
+    fpsdk::common::parser::nmea::NmeaFloat std_lat_;
+    fpsdk::common::parser::nmea::NmeaFloat std_lon_;
+    fpsdk::common::parser::nmea::NmeaFloat std_alt_;
+    fpsdk::common::parser::nmea::NmeaFloat pdop_;
+    fpsdk::common::parser::nmea::NmeaFloat hdop_;
+    fpsdk::common::parser::nmea::NmeaFloat vdop_;
+    fpsdk::common::parser::nmea::NmeaFloat heading_;
+    fpsdk::common::parser::nmea::NmeaFloat speed_;
+    fpsdk::common::parser::nmea::NmeaFloat course_;
+    fpsdk::common::parser::nmea::NmeaFloat cogt_;
+    fpsdk::common::parser::nmea::NmeaFloat cogm_;
+    fpsdk::common::parser::nmea::NmeaFloat sogn_;
+    fpsdk::common::parser::nmea::NmeaFloat sogk_;
+    fpsdk::common::parser::nmea::NmeaFloat diff_age_;
+    fpsdk::common::parser::nmea::NmeaInt diff_sta_;
+    fpsdk::common::parser::nmea::NmeaInt local_hr_;
+    fpsdk::common::parser::nmea::NmeaInt local_min_;
+
+    Eigen::Matrix<double, 3, 3> cov_enu_;
+
+    // Completes the data and returns it, and resets the instance back to empty
+    NmeaEpochData CompleteAndReset();
+};
+
+void HelloWorld();
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-#endif  // __FIXPOSITION_DRIVER_LIB_HELPER__
+#endif  // __FIXPOSITION_DRIVER_LIB_HELPER_HPP__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp
deleted file mode 100644
index 8c88650..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/base_converter.hpp
+++ /dev/null
@@ -1,268 +0,0 @@
-/**
- *  @file
- *  @brief Base Converter to define the interfaces
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__
-#define __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__
-
-/* SYSTEM / STL */
-#include <iostream>
-#include <vector>
-
-/* PACKAGE */
-#include <fixposition_driver_lib/time_conversions.hpp>
-
-namespace fixposition {
-
-class BaseAsciiConverter {
-    public:
-        BaseAsciiConverter() = default;
-        ~BaseAsciiConverter() = default;
-
-        /**
-         * @brief Virtual interface to convert the split tokens into ros messages
-         *
-         * @param[in] tokens vector of strings split by comma
-         */
-        virtual void ConvertTokens(const std::vector<std::string>& tokens) = 0;
-};
-
-template <typename MessageType> class NmeaConverter : public BaseAsciiConverter {
-    public:
-        using Observer = std::function<void(const MessageType&)>;
-
-        /**
-         * @brief Construct a new Fixposition Msg Converter object
-         *
-         */
-        
-        NmeaConverter() : BaseAsciiConverter() {}
-
-        ~NmeaConverter() = default;
-
-        /**
-         * @brief Comma Delimited FP_A message, convert to Data structs and, if available, call observers
-         *
-         * @param[in] state state message as string
-         * @return FP_A message struct
-         */
-        void ConvertTokens(const std::vector<std::string>& tokens) {
-            msg_.ConvertFromTokens(tokens);
-
-            // process all observers
-            for (auto& ob : obs_) {
-                ob(msg_);
-            }
-        };
-
-        /**
-         * @brief Add Observer to call at the end of ConvertTokens()
-         *
-         * @param[in] ob
-         */
-        void AddObserver(Observer ob) { obs_.push_back(ob); }
-
-    private:
-        MessageType msg_;
-        std::vector<Observer> obs_;
-};
-
-//===================================================
-
-/**
- * @brief Parse status flag field
- *
- * @param[in] tokens list of tokens
- * @param[in] idx status flag index
- * @return int
- */
-inline int ParseStatusFlag(const std::vector<std::string>& tokens, const int idx) {
-    if (tokens.at(idx).empty()) {
-        return -1;
-    } else {
-        return std::stoi(tokens.at(idx));
-    }
-}
-
-/**
- * @brief Helper function to convert string into int. If string is empty then 0 is returned
- *
- * @param[in] in_str
- * @return int
- */
-inline double StringToInt(const std::string& in_str) { return in_str.empty() ? 0 : std::stoi(in_str); }
-
-/**
- * @brief Helper function to convert string into double. If string is empty then 0.0 is returned
- *
- * @param[in] in_str
- * @return double
- */
-inline double StringToDouble(const std::string& in_str) { return in_str.empty() ? 0. : std::stod(in_str); }
-
-/**
- * @brief Helper function to convert string into char. If string is empty then '0' is returned
- *
- * @param[in] in_str
- * @return int
- */
-inline double StringToChar(const std::string& in_str) { return in_str.empty() ? 0 : in_str[0]; }
-
-/**
- * @brief Make sure the quaternion is unit quaternion
- *
- * @param[in] quat Eigen::Quaterniond to be checked
- * @return true valid quaternion
- * @return false invalid quaternion
- */
-inline bool CheckQuat(const Eigen::Quaterniond& quat) { return abs(quat.squaredNorm() - 1.0) <= 1e-3; }
-
-/**
- * @brief Convert 3 string values into a Eigen::Vector3d
- *
- * @param[in] x
- * @param[in] y
- * @param[in] z
- * @return Eigen::Vector3d
- */
-inline Eigen::Vector3d Vector3ToEigen(const std::string& x, const std::string& y, const std::string& z) {
-    return Eigen::Vector3d(StringToDouble(x), StringToDouble(y), StringToDouble(z));
-}
-
-/**
- * @brief convert 4 string values into a Eigen::Quaterniond
- *
- * @param[in] w
- * @param[in] x
- * @param[in] y
- * @param[in] z
- * @return Eigen::Quaterniond
- */
-inline Eigen::Quaterniond Vector4ToEigen(const std::string& w, const std::string& x, const std::string& y,
-                                         const std::string& z) {
-    return Eigen::Quaterniond(StringToDouble(w), StringToDouble(x), StringToDouble(y), StringToDouble(z));
-}
-
-/**
- * @brief Helper function to convert strings containing gps_wno and gps_tow into ros::Time
- *
- * @param[in] gps_wno
- * @param[in] gps_tow
- * @return ros::Time
- */
-inline times::GpsTime ConvertGpsTime(const std::string& gps_wno, const std::string& gps_tow) {
-    if (!gps_wno.empty() && !gps_tow.empty()) {
-        return times::GpsTime(std::stoi(gps_wno), std::stod(gps_tow));
-    } else {
-        return times::GpsTime(0, 0);
-    }
-}
-
-/**
- * @brief Build a 3x3 covariance matrix
- *
- * [xx, xy, xz,
- *  xy, yy, yz,
- *  xz, yz, zz]
- *
- * @param[in] xx
- * @param[in] yy
- * @param[in] zz
- * @param[in] xy
- * @param[in] yz
- * @param[in] xz
- * @return Eigen::Matrix<double, 3, 3> the 3x3 matrix
- */
-inline Eigen::Matrix<double, 3, 3> BuildCovMat3D(const double xx, const double yy, const double zz, 
-                                                 const double xy, const double yz, const double xz) {
-    Eigen::Matrix<double, 3, 3> cov;
-    cov.setZero();
-
-    // Diagonals
-    cov(0, 0) = xx;   // 0
-    cov(1, 1) = yy;   // 4
-    cov(2, 2) = zz;   // 8
-
-    // Rest of values
-    cov(1, 0) = cov(0, 1) = xy; // 1 = 3
-    cov(2, 1) = cov(1, 2) = yz; // 2 = 6
-    cov(2, 0) = cov(0, 2) = xz; // 5 = 7
-
-    return cov;
-}
-
-/**
- * @brief Build a 6x6 covariance matrix which is 2 independent 3x3 matrices
- *
- * [xx, xy, xz, 0, 0, 0,
- *  xy, yy, yz, 0, 0, 0,
- *  xz, yz, zz, 0, 0, 0,
- *  0, 0, 0, xx1, xy1, xz1,
- *  0, 0, 0, xy1, yy1, yz1,
- *  0, 0, 0, xz1, yz1, zz1]
- *
- * @param[in] xx
- * @param[in] yy
- * @param[in] zz
- * @param[in] xy
- * @param[in] yz
- * @param[in] xz
- * @param[in] xx1
- * @param[in] yy1
- * @param[in] zz1
- * @param[in] xy1
- * @param[in] yz1
- * @param[in] xz1
- * @return Eigen::Matrix<double, 6, 6> the 6x6 matrix
- */
-inline Eigen::Matrix<double, 6, 6> BuildCovMat6D(const double xx, const double yy, const double zz, const double xy,
-                                                 const double yz, const double xz, double xx1, const double yy1,
-                                                 const double zz1, const double xy1, const double yz1, double xz1) {
-    Eigen::Matrix<double, 6, 6> cov;
-    cov.setZero();
-
-    // Diagonals
-    cov(0, 0) = xx;   // 0
-    cov(1, 1) = yy;   // 7
-    cov(2, 2) = zz;   // 14
-    cov(3, 3) = xx1;  // 21
-    cov(4, 4) = yy1;  // 28
-    cov(5, 5) = zz1;  // 35
-
-    // Rest of values
-    cov(1, 0) = cov(0, 1) = xy;   // 1 = 6
-    cov(2, 1) = cov(1, 2) = yz;   // 8 = 13
-    cov(2, 0) = cov(0, 2) = xz;   // 2 = 12
-    cov(4, 3) = cov(3, 4) = xy1;  // 22 = 27
-    cov(5, 4) = cov(4, 5) = yz1;  // 29 = 34
-    cov(5, 3) = cov(3, 5) = xz1;  // 23 = 33
-
-    return cov;
-}
-
-/**
- * @brief Convert radians to degrees
- *
- * @tparam T data type
- * @param radians angle in [rad]
- *
- * @returns the angle in [deg]
- */
-template <typename T>
-constexpr inline T RadToDeg(T radians) {
-    static_assert(std::is_floating_point<T>::value, "Only floating point types allowed!");
-    return radians * 57.295779513082320876798154814105;
-}
-
-}  // namespace fixposition
-#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_BASE_CONVERTER__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp
deleted file mode 100644
index 59609f3..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpa_type.hpp
+++ /dev/null
@@ -1,644 +0,0 @@
-/**
- *  @file
- *  @brief Declaration of FP_A type messages
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE__
-#define __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE__
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/msg_data.hpp>
-#include <fixposition_driver_lib/time_conversions.hpp>
-
-namespace fixposition {
-
-// ------------ FP_A-ODOMETRY ------------
-
-struct FP_ODOMETRY {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    OdometryData odom;
-    Eigen::Vector3d acceleration;
-    int fusion_status;
-    int imu_bias_status;
-    int gnss1_status;
-    int gnss2_status;
-    int wheelspeed_status;
-    std::string version;
-    
-    // Message structure
-    const std::string frame_id = "FP_ECEF";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "ODOMETRY";
-    static constexpr unsigned int kVersion_ = 2;
-    static constexpr unsigned int kSize_ = 45;
-
-    FP_ODOMETRY() {
-        odom.stamp = fixposition::times::GpsTime();
-        odom.frame_id = frame_id;
-        odom.child_frame_id = child_frame_id;
-        odom.pose = PoseWithCovData();
-        odom.twist = TwistWithCovData();
-        acceleration.setZero();
-        fusion_status = -1;
-        imu_bias_status = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        wheelspeed_status = -1;
-        version = "Unknown";
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        odom.stamp = fixposition::times::GpsTime();
-        odom.frame_id = frame_id;
-        odom.child_frame_id = child_frame_id;
-        odom.pose = PoseWithCovData();
-        odom.twist = TwistWithCovData();
-        acceleration.setZero();
-        fusion_status = -1;
-        imu_bias_status = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        wheelspeed_status = -1;
-        version = "Unknown";
-    }
-};
-
-// ------------ FP_A-ODOMENU ------------
-
-struct FP_ODOMENU {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    OdometryData odom;
-    Eigen::Vector3d acceleration;
-    int fusion_status;
-    int imu_bias_status;
-    int gnss1_status;
-    int gnss2_status;
-    int wheelspeed_status;
-
-    // Message structure
-    const std::string frame_id = "FP_ENU0";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "ODOMENU";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 44;
-
-    FP_ODOMENU() {
-        odom.stamp = fixposition::times::GpsTime();
-        odom.frame_id = frame_id;
-        odom.child_frame_id = child_frame_id;
-        odom.pose = PoseWithCovData();
-        odom.twist = TwistWithCovData();
-        acceleration.setZero();
-        fusion_status = -1;
-        imu_bias_status = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        wheelspeed_status = -1;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        odom.stamp = fixposition::times::GpsTime();
-        odom.frame_id = frame_id;
-        odom.child_frame_id = child_frame_id;
-        odom.pose = PoseWithCovData();
-        odom.twist = TwistWithCovData();
-        acceleration.setZero();
-        fusion_status = -1;
-        imu_bias_status = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        wheelspeed_status = -1;
-    }
-};
-
-// ------------ FP_A-ODOMSH ------------
-
-struct FP_ODOMSH {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    OdometryData odom;
-    Eigen::Vector3d acceleration;
-    int fusion_status;
-    int imu_bias_status;
-    int gnss1_status;
-    int gnss2_status;
-    int wheelspeed_status;
-
-    // Message structure
-    const std::string frame_id = "FP_ECEF";
-    const std::string child_frame_id = "FP_POISH";
-    const std::string header_ = "ODOMSH";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 44;
-
-    FP_ODOMSH() {
-        odom.stamp = fixposition::times::GpsTime();
-        odom.frame_id = frame_id;
-        odom.child_frame_id = child_frame_id;
-        odom.pose = PoseWithCovData();
-        odom.twist = TwistWithCovData();
-        acceleration.setZero();
-        fusion_status = -1;
-        imu_bias_status = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        wheelspeed_status = -1;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        odom.stamp = fixposition::times::GpsTime();
-        odom.frame_id = frame_id;
-        odom.child_frame_id = child_frame_id;
-        odom.pose = PoseWithCovData();
-        odom.twist = TwistWithCovData();
-        acceleration.setZero();
-        fusion_status = -1;
-        imu_bias_status = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        wheelspeed_status = -1;
-    }
-};
-
-// ------------ FP_A-ODOMSTATUS ------------
-
-struct FP_ODOMSTATUS {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    times::GpsTime stamp;
-    int init_status;
-    int fusion_imu;
-    int fusion_gnss1;
-    int fusion_gnss2;
-    int fusion_corr;
-    int fusion_cam1;
-    int fusion_ws;
-    int fusion_markers;
-    int imu_status;
-    int imu_noise;
-    int imu_conv;
-    int gnss1_status;
-    int gnss2_status;
-    int baseline_status;
-    int corr_status;
-    int cam1_status;
-    int ws_status;
-    int ws_conv;
-    int markers_status;
-    int markers_conv;
-
-    // Message structure
-    const std::string header_ = "ODOMSTATUS";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 41;
-
-    FP_ODOMSTATUS() {
-        stamp = fixposition::times::GpsTime();
-        init_status = -1;
-        fusion_imu = -1;
-        fusion_gnss1 = -1;
-        fusion_gnss2 = -1;
-        fusion_corr = -1;
-        fusion_cam1 = -1;
-        fusion_ws = -1;
-        fusion_markers = -1;
-        imu_status = -1;
-        imu_noise = -1;
-        imu_conv = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        baseline_status = -1;
-        corr_status = -1;
-        cam1_status = -1;
-        ws_status = -1;
-        ws_conv = -1;
-        markers_status = -1;
-        markers_conv = -1;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        stamp = fixposition::times::GpsTime();
-        init_status = -1;
-        fusion_imu = -1;
-        fusion_gnss1 = -1;
-        fusion_gnss2 = -1;
-        fusion_corr = -1;
-        fusion_cam1 = -1;
-        fusion_ws = -1;
-        fusion_markers = -1;
-        imu_status = -1;
-        imu_noise = -1;
-        imu_conv = -1;
-        gnss1_status = -1;
-        gnss2_status = -1;
-        baseline_status = -1;
-        corr_status = -1;
-        cam1_status = -1;
-        ws_status = -1;
-        ws_conv = -1;
-        markers_status = -1;
-        markers_conv = -1;
-    }
-};
-
-// ------------ FP_A-LLH ------------
-
-struct FP_LLH {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    times::GpsTime stamp;    
-    Eigen::Vector3d llh;
-    Eigen::Matrix<double, 3, 3> cov;
-
-    // Message structure
-    const std::string frame_id = "FP_LLH";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "LLH";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 14;
-
-    FP_LLH() {
-        stamp = fixposition::times::GpsTime();
-        llh.setZero();
-        cov.setZero();
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        stamp = fixposition::times::GpsTime();
-        llh.setZero();
-        cov.setZero();
-    }
-};
-
-// ------------ FP_A-TF ------------
-
-struct FP_TF {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    TfData tf;
-    bool valid_tf;
-
-    // Message structure
-    const std::string header_ = "TF";
-    static constexpr unsigned int kVersion_ = 2;
-    static constexpr unsigned int kSize_ = 14;
-
-    FP_TF() {
-        tf.stamp = fixposition::times::GpsTime();
-        tf.frame_id = "";
-        tf.child_frame_id = "";
-        tf.translation.setZero();
-        tf.rotation.setIdentity();
-        valid_tf = false;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        tf.stamp = fixposition::times::GpsTime();
-        tf.frame_id = "";
-        tf.child_frame_id = "";
-        tf.translation.setZero();
-        tf.rotation.setIdentity();
-        valid_tf = false;
-    }
-};
-
-// ------------ FP_A-RAWIMU ------------
-
-struct FP_RAWIMU {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    ImuData imu;
-
-    // Message structure
-    const std::string header_ = "RAWIMU";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 11;
-
-    FP_RAWIMU() {
-        imu.stamp = fixposition::times::GpsTime();
-        imu.frame_id = "";
-        imu.linear_acceleration.setZero();
-        imu.angular_velocity.setZero();
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        imu.stamp = fixposition::times::GpsTime();
-        imu.frame_id = "";
-        imu.linear_acceleration.setZero();
-        imu.angular_velocity.setZero();
-    }
-};
-
-// ------------ FP_A-IMUBIAS ------------
-
-struct FP_IMUBIAS {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    times::GpsTime stamp;
-    std::string frame_id;
-    int fusion_imu;
-    int imu_status;
-    int imu_noise;
-    int imu_conv;
-    Eigen::Vector3d bias_acc;
-    Eigen::Vector3d bias_gyr;
-    Eigen::Vector3d bias_cov_acc;
-    Eigen::Vector3d bias_cov_gyr;
-
-    // Message structure
-    const std::string header_ = "IMUBIAS";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 21;
-
-    FP_IMUBIAS() {
-        stamp = fixposition::times::GpsTime();
-        frame_id = "";
-        fusion_imu = -1;
-        imu_status = -1;
-        imu_noise = -1;
-        imu_conv = -1;
-        bias_acc.setZero();
-        bias_gyr.setZero();
-        bias_cov_acc.setZero();
-        bias_cov_gyr.setZero();
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        stamp = fixposition::times::GpsTime();
-        frame_id = "";
-        fusion_imu = -1;
-        imu_status = -1;
-        imu_noise = -1;
-        imu_conv = -1;
-        bias_acc.setZero();
-        bias_gyr.setZero();
-        bias_cov_acc.setZero();
-        bias_cov_gyr.setZero();
-    }
-};
-
-// ------------ FP_A-CORRIMU ------------
-
-struct FP_CORRIMU {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    ImuData imu;
-
-    // Message structure
-    const std::string header_ = "CORRIMU";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 11;
-
-    FP_CORRIMU() {
-        imu.stamp = fixposition::times::GpsTime();
-        imu.frame_id = "";
-        imu.linear_acceleration.setZero();
-        imu.angular_velocity.setZero();
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        imu.stamp = fixposition::times::GpsTime();
-        imu.frame_id = "";
-        imu.linear_acceleration.setZero();
-        imu.angular_velocity.setZero();
-    }
-};
-
-// ------------ FP_A-GNSSANT ------------
-
-struct FP_GNSSANT {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    times::GpsTime stamp;
-    std::string gnss1_state;
-    std::string gnss1_power;
-    int gnss1_age;
-    std::string gnss2_state;
-    std::string gnss2_power;
-    int gnss2_age;
-
-    // Message structure
-    const std::string header_ = "GNSSANT";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 11;
-
-    FP_GNSSANT() {
-        stamp = fixposition::times::GpsTime();
-        gnss1_state = "";
-        gnss1_power = "";
-        gnss1_age = -1;
-        gnss2_state = "";
-        gnss2_power = "";
-        gnss2_age = -1;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        stamp = fixposition::times::GpsTime();
-        gnss1_state = "";
-        gnss1_power = "";
-        gnss1_age = -1;
-        gnss2_state = "";
-        gnss2_power = "";
-        gnss2_age = -1;
-    }
-};
-
-// ------------ FP_A-GNSSCORR ------------
-
-struct FP_GNSSCORR {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    times::GpsTime stamp;
-    int gnss1_fix;
-    int gnss1_nsig_l1;
-    int gnss1_nsig_l2;
-    int gnss2_fix;
-    int gnss2_nsig_l1;
-    int gnss2_nsig_l2;
-    double corr_latency;
-    double corr_update_rate;
-    double corr_data_rate;
-    double corr_msg_rate;
-    int sta_id;
-    Eigen::Vector3d sta_llh;
-    int sta_dist;
-
-    // Message structure
-    const std::string header_ = "GNSSCORR";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 20;
-
-    FP_GNSSCORR() {
-        stamp = fixposition::times::GpsTime();
-        gnss1_fix = -1;
-        gnss1_nsig_l1 = -1;
-        gnss1_nsig_l2 = -1;
-        gnss2_fix = -1;
-        gnss2_nsig_l1 = -1;
-        gnss2_nsig_l2 = -1;
-        corr_latency = -1.0;
-        corr_update_rate = -1.0;
-        corr_data_rate = -1.0;
-        corr_msg_rate = -1.0;
-        sta_id = -1;
-        sta_llh.setZero();
-        sta_dist = -1;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        stamp = fixposition::times::GpsTime();
-        gnss1_fix = -1;
-        gnss1_nsig_l1 = -1;
-        gnss1_nsig_l2 = -1;
-        gnss2_fix = -1;
-        gnss2_nsig_l1 = -1;
-        gnss2_nsig_l2 = -1;
-        corr_latency = -1.0;
-        corr_update_rate = -1.0;
-        corr_data_rate = -1.0;
-        corr_msg_rate = -1.0;
-        sta_id = -1;
-        sta_llh.setZero();
-        sta_dist = -1;
-    }
-};
-
-// ------------ FP_A-TEXT ------------
-
-struct FP_TEXT {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    std::string level;
-    std::string text;
-
-    // Message structure
-    const std::string header_ = "TEXT";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 5;
-
-    FP_TEXT() {
-        level = "";
-        text = "";
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        level = "";
-        text = "";
-    }
-};
-
-// ------------ FP_A-EOE ------------
-
-struct FP_EOE {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    times::GpsTime stamp;
-    std::string epoch;
-
-    // Message structure
-    const std::string header_ = "EOE";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 6;
-
-    FP_EOE() {
-        stamp = fixposition::times::GpsTime();
-        epoch = "";
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        stamp = fixposition::times::GpsTime();
-        epoch = "";
-    }
-};
-
-// ------------ FP_A-TP ------------
-
-struct FP_TP {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    
-    // Message fields
-    std::string tp_name;
-    std::string timebase;
-    std::string timeref;
-    int tp_tow_sec;
-    double tp_tow_psec;
-    int gps_leaps;
-
-    // Message structure
-    const std::string header_ = "TP";
-    static constexpr unsigned int kVersion_ = 1;
-    static constexpr unsigned int kSize_ = 9;
-
-    FP_TP() {
-        tp_name = "";
-        timebase = "";
-        timeref = "";
-        tp_tow_sec = 0;
-        tp_tow_psec = 0.0;
-        gps_leaps = 0;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        tp_name = "";
-        timebase = "";
-        timeref = "";
-        tp_tow_sec = 0;
-        tp_tow_psec = 0.0;
-        gps_leaps = 0;
-    }
-};
-
-}  // namespace fixposition
-#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_FPA_TYPE__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp
deleted file mode 100644
index f7edd36..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_measurements.hpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/**
- *  @file
- *  @brief Declaration of FpbMeasurementsMeas message struct
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__
-#define __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__
-
-/* EXTERNAL */
-#include <stdint.h>
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpb_type.hpp>
-
-namespace fixposition {
-
-struct FpbMeasurementsHeader {
-    uint8_t  version;       //!< Message version (= FpbMeasurementsVersion for the current version of this message)
-    uint8_t  num_meas;      //!< Number of measurements in the body (1..FP_B_MEASUREMENTS_MAX_NUM_MEAS)
-    uint8_t  reserved0[6];  //!< Reserved for future use. Set to 0.
-};
-static_assert(sizeof(FpbMeasurementsHeader) == 8, "");
-const int FP_B_MEASUREMENTS_HEAD_SIZE = 8;
-
-enum FpbMeasurementsMeasType {
-    MEASTYPE_UNSPECIFIED = 0,  //!< Message type unspecified, measurement will not be processed
-    MEASTYPE_VELOCITY    = 1,  //!< Velocity message, measurement will be interpreted as a wheel speed
-};
-
-enum FpbMeasurementsMeasLoc {
-    MEASLOC_UNSPECIFIED = 0,  //!< Location of the sensor unspecified, measurement will not be processed
-    MEASLOC_RC          = 1,  //!< Measurement coming from the RC sensor
-    MEASLOC_FR          = 2,  //!< Measurement coming from the FR sensor
-    MEASLOC_FL          = 3,  //!< Measurement coming from the FL sensor
-    MEASLOC_RR          = 4,  //!< Measurement coming from the RR sensor
-    MEASLOC_RL          = 5,  //!< Measurement coming from the RL sensor
-};
-
-enum FpbMeasurementsTimestampType {
-    TIME_UNSPECIFIED = 0,  //!< Timestamp type unspecified, measurement will not be processed
-    TIME_TOA         = 1,  //!< Use time of arrival of the measurement
-};
-
-struct FpbMeasurementsMeas {
-    int32_t  meas_x;          //!< Measurement x
-    int32_t  meas_y;          //!< Measurement y
-    int32_t  meas_z;          //!< Measurement z
-    uint8_t  meas_x_valid;    //!< Validity of measurement x (1 = meas_x contains valid data, 0 = data invalid or n/a)
-    uint8_t  meas_y_valid;    //!< Validity of measurement y (1 = meas_x contains valid data, 0 = data invalid or n/a)
-    uint8_t  meas_z_valid;    //!< Validity of measurement z (1 = meas_x contains valid data, 0 = data invalid or n/a)
-    uint8_t  meas_type;       //!< See FpbMeasurementsMeasType
-    uint8_t  meas_loc;        //!< See FpbMeasurementsMeasLoc
-    uint8_t  reserved1[4];    //!< Reserved for future use. Set to 0.
-    uint8_t  timestamp_type;  //!< See FpbMeasurementsTimestampType
-    uint16_t gps_wno;         //!< GPS week number [-]
-    uint32_t gps_tow;         //!< GPS time of week [ms] or monotonic time [-]
-};
-
-static_assert(sizeof(FpbMeasurementsMeas) == 28, "");
-const int FP_B_MEASUREMENTS_BODY_SIZE = 28;
-
-}  // namespace fixposition
-
-#endif  // __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp
deleted file mode 100644
index 02417cc..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/fpb_type.hpp
+++ /dev/null
@@ -1,92 +0,0 @@
-/**
- *  @file
- *  @brief Declaration of Fpb message framework
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_FPB__
-#define __FIXPOSITION_DRIVER_LIB_FPB__
-
-/* SYSTEM / STL */
-#include <stdint.h>
-
-namespace fixposition {
-
-static uint32_t const k_crc32_fpb[] = {
-    0x00000000, 0x32c00699, 0x65800d32, 0x57400bab, 0xcb001a64, 0xf9c01cfd,
-    0xae801756, 0x9c4011cf, 0xa4c03251, 0x960034c8, 0xc1403f63, 0xf38039fa,
-    0x6fc02835, 0x5d002eac, 0x0a402507, 0x3880239e, 0x7b40623b, 0x498064a2,
-    0x1ec06f09, 0x2c006990, 0xb040785f, 0x82807ec6, 0xd5c0756d, 0xe70073f4,
-    0xdf80506a, 0xed4056f3, 0xba005d58, 0x88c05bc1, 0x14804a0e, 0x26404c97,
-    0x7100473c, 0x43c041a5, 0xf680c476, 0xc440c2ef, 0x9300c944, 0xa1c0cfdd,
-    0x3d80de12, 0x0f40d88b, 0x5800d320, 0x6ac0d5b9, 0x5240f627, 0x6080f0be,
-    0x37c0fb15, 0x0500fd8c, 0x9940ec43, 0xab80eada, 0xfcc0e171, 0xce00e7e8,
-    0x8dc0a64d, 0xbf00a0d4, 0xe840ab7f, 0xda80ade6, 0x46c0bc29, 0x7400bab0,
-    0x2340b11b, 0x1180b782, 0x2900941c, 0x1bc09285, 0x4c80992e, 0x7e409fb7,
-    0xe2008e78, 0xd0c088e1, 0x8780834a, 0xb54085d3, 0xdfc18e75, 0xed0188ec,
-    0xba418347, 0x888185de, 0x14c19411, 0x26019288, 0x71419923, 0x43819fba,
-    0x7b01bc24, 0x49c1babd, 0x1e81b116, 0x2c41b78f, 0xb001a640, 0x82c1a0d9,
-    0xd581ab72, 0xe741adeb, 0xa481ec4e, 0x9641ead7, 0xc101e17c, 0xf3c1e7e5,
-    0x6f81f62a, 0x5d41f0b3, 0x0a01fb18, 0x38c1fd81, 0x0041de1f, 0x3281d886,
-    0x65c1d32d, 0x5701d5b4, 0xcb41c47b, 0xf981c2e2, 0xaec1c949, 0x9c01cfd0,
-    0x29414a03, 0x1b814c9a, 0x4cc14731, 0x7e0141a8, 0xe2415067, 0xd08156fe,
-    0x87c15d55, 0xb5015bcc, 0x8d817852, 0xbf417ecb, 0xe8017560, 0xdac173f9,
-    0x46816236, 0x744164af, 0x23016f04, 0x11c1699d, 0x52012838, 0x60c12ea1,
-    0x3781250a, 0x05412393, 0x9901325c, 0xabc134c5, 0xfc813f6e, 0xce4139f7,
-    0xf6c11a69, 0xc4011cf0, 0x9341175b, 0xa18111c2, 0x3dc1000d, 0x0f010694,
-    0x58410d3f, 0x6a810ba6, 0x8d431a73, 0xbf831cea, 0xe8c31741, 0xda0311d8,
-    0x46430017, 0x7483068e, 0x23c30d25, 0x11030bbc, 0x29832822, 0x1b432ebb,
-    0x4c032510, 0x7ec32389, 0xe2833246, 0xd04334df, 0x87033f74, 0xb5c339ed,
-    0xf6037848, 0xc4c37ed1, 0x9383757a, 0xa14373e3, 0x3d03622c, 0x0fc364b5,
-    0x58836f1e, 0x6a436987, 0x52c34a19, 0x60034c80, 0x3743472b, 0x058341b2,
-    0x99c3507d, 0xab0356e4, 0xfc435d4f, 0xce835bd6, 0x7bc3de05, 0x4903d89c,
-    0x1e43d337, 0x2c83d5ae, 0xb0c3c461, 0x8203c2f8, 0xd543c953, 0xe783cfca,
-    0xdf03ec54, 0xedc3eacd, 0xba83e166, 0x8843e7ff, 0x1403f630, 0x26c3f0a9,
-    0x7183fb02, 0x4343fd9b, 0x0083bc3e, 0x3243baa7, 0x6503b10c, 0x57c3b795,
-    0xcb83a65a, 0xf943a0c3, 0xae03ab68, 0x9cc3adf1, 0xa4438e6f, 0x968388f6,
-    0xc1c3835d, 0xf30385c4, 0x6f43940b, 0x5d839292, 0x0ac39939, 0x38039fa0,
-    0x52829406, 0x6042929f, 0x37029934, 0x05c29fad, 0x99828e62, 0xab4288fb,
-    0xfc028350, 0xcec285c9, 0xf642a657, 0xc482a0ce, 0x93c2ab65, 0xa102adfc,
-    0x3d42bc33, 0x0f82baaa, 0x58c2b101, 0x6a02b798, 0x29c2f63d, 0x1b02f0a4,
-    0x4c42fb0f, 0x7e82fd96, 0xe2c2ec59, 0xd002eac0, 0x8742e16b, 0xb582e7f2,
-    0x8d02c46c, 0xbfc2c2f5, 0xe882c95e, 0xda42cfc7, 0x4602de08, 0x74c2d891,
-    0x2382d33a, 0x1142d5a3, 0xa4025070, 0x96c256e9, 0xc1825d42, 0xf3425bdb,
-    0x6f024a14, 0x5dc24c8d, 0x0a824726, 0x384241bf, 0x00c26221, 0x320264b8,
-    0x65426f13, 0x5782698a, 0xcbc27845, 0xf9027edc, 0xae427577, 0x9c8273ee,
-    0xdf42324b, 0xed8234d2, 0xbac23f79, 0x880239e0, 0x1442282f, 0x26822eb6,
-    0x71c2251d, 0x43022384, 0x7b82001a, 0x49420683, 0x1e020d28, 0x2cc20bb1,
-    0xb0821a7e, 0x82421ce7, 0xd502174c, 0xe7c211d5
-};
-
-inline uint32_t Crc32fpb(const uint8_t* data, const int size) {
-    uint32_t crc = 0;
-    if (data != nullptr) {
-        for (int ix = 0; ix < size; ix++) {
-            crc = (crc << 8) ^ k_crc32_fpb[((crc >> 24) ^ data[ix]) & 0xff];
-        }
-    }
-    return crc;
-}
-
-struct FpbHeader {
-    uint8_t  sync1;         //!< FP_B frame sync char 1 (0x66)
-    uint8_t  sync2;         //!< FP_B frame sync char 2 (0x21)
-    uint16_t msg_id;        //!< ID of the FP_B message (2001 for measurements message)
-    uint16_t payload_size;  //!< Size of the payload
-    uint16_t time;          //!< Time of the message. Unused, set to 0.
-};
-static_assert(sizeof(FpbHeader) == 8, "");
-const int FP_B_HEAD_SIZE = 8;  //!< Size of FP_B frame header
-const int FP_B_CRC_SIZE = 4;   //!< Size of FP_B crc
-
-}  // namespace fixposition
-
-#endif  // __FIXPOSITION_DRIVER_LIB_FPB__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp
deleted file mode 100644
index 3eb58ab..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/msg_data.hpp
+++ /dev/null
@@ -1,190 +0,0 @@
-/**
- *  @file
- *  @brief Declaration of Data types
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_MSG_DATA__
-#define __FIXPOSITION_DRIVER_LIB_MSG_DATA__
-
-/* SYSTEM / STL */
-#include <unordered_map>
-
-/* PACKAGE */
-#include <fixposition_driver_lib/time_conversions.hpp>
-
-namespace fixposition {
-
-enum class FusionStatus : int {
-    NOT_STARTED = 0,
-    VISION_ONLY = 1,
-    VISUAL_INERTIAL_FUSION = 2,
-    INERTIAL_GNSS_FUSION = 3,
-    VISUAL_INERTIAL_GNSS_FUSION = 4,
-};
-
-enum class ImuStatus : int {
-    NOT_CONVERGED = 0,
-    BIAS_CONVERGED = 1,
-};
-
-enum class GnssStatus : int {
-    FIX_TYPE_UNKNOWN = 0,
-    FIX_TYPE_NOFIX = 1,
-    FIX_TYPE_DRONLY = 2,
-    FIX_TYPE_TIME = 3,
-    FIX_TYPE_S2D = 4,
-    FIX_TYPE_S3D = 5,
-    FIX_TYPE_S3D_DR = 6,
-    FIX_TYPE_RTK_FLOAT = 7,
-    FIX_TYPE_RTK_FIXED = 8,
-    FIX_TYPE_RTK_FLOAT_DR = 9,
-    FIX_TYPE_RTK_FIXED_DR = 10,
-};
-
-enum class WheelspeedStatus : int {
-    NOT_ENABLED = -1,
-    NOT_CONVERGED = 0,
-    WS_CONVERGED = 1,
-};
-
-enum class SignalType : uint8_t { Invalid, GPS, Galileo, BeiDou, GLONASS };
-
-struct ImuData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    times::GpsTime stamp;
-    std::string frame_id;
-    Eigen::Vector3d linear_acceleration;
-    Eigen::Vector3d angular_velocity;
-    ImuData() {
-        linear_acceleration.setZero();
-        angular_velocity.setZero();
-    }
-};
-
-struct TfData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    times::GpsTime stamp;
-    std::string frame_id;
-    std::string child_frame_id;
-    Eigen::Vector3d translation;
-    Eigen::Quaterniond rotation;
-    TfData() : frame_id(""), child_frame_id("") {
-        translation.setZero();
-        rotation.setIdentity();
-    }
-};
-
-struct PoseWithCovData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    Eigen::Vector3d position;
-    Eigen::Quaterniond orientation;
-    Eigen::Matrix<double, 6, 6> cov;
-    PoseWithCovData() {
-        position.setZero();
-        orientation.setIdentity();
-        cov.setZero();
-    }
-};
-
-struct TwistWithCovData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    Eigen::Vector3d linear;
-    Eigen::Vector3d angular;
-    Eigen::Matrix<double, 6, 6> cov;
-    TwistWithCovData() {
-        linear.setZero();
-        angular.setZero();
-        cov.setZero();
-    }
-};
-
-struct OdometryData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    times::GpsTime stamp;
-    std::string frame_id;
-    std::string child_frame_id;
-    PoseWithCovData pose;
-    TwistWithCovData twist;
-};
-
-struct NavSatStatusData {
-    enum class Status : int8_t {
-        STATUS_NO_FIX = -1,   // Unable to fix position
-        STATUS_FIX = 0,       // Unaugmented fix
-        STATUS_SBAS_FIX = 1,  // With satellite-based augmentation
-        STATUS_GBAS_FIX = 2,  // With ground-based augmentation
-    };
-
-    enum class Service : uint16_t {
-        SERVICE_NONE = 0,
-        SERVICE_GPS = 1,
-        SERVICE_GLONASS = 2,
-        SERVICE_COMPASS = 4,  // includes BeiDou.
-        SERVICE_GALILEO = 8,
-        SERVICE_ALL = 15,
-    };
-
-    int8_t status;
-    uint16_t service;
-    NavSatStatusData() : status(static_cast<int8_t>(Status::STATUS_NO_FIX)), service(0) {}
-};
-
-struct NavSatFixData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    times::GpsTime stamp;
-    std::string frame_id;
-    NavSatStatusData status;
-    double latitude;
-    double longitude;
-    double altitude;
-    Eigen::Matrix<double, 3, 3> cov;
-    int position_covariance_type;
-    NavSatFixData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); }
-};
-
-struct GpggaData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    std::string time;
-    double latitude;
-    double longitude;
-    double altitude;
-    Eigen::Matrix<double, 3, 3> cov;
-    int position_covariance_type;
-    bool valid;
-    GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0), valid(false) {
-        cov.setZero();
-    }
-};
-
-struct GpzdaData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    std::string time;
-    std::string date;
-    times::GpsTime stamp;
-    bool valid;
-    GpzdaData() : time(""), date(""), valid(false) {}
-};
-
-struct GprmcData {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    std::string time;
-    std::string mode;
-    double latitude;
-    double longitude;
-    double speed;
-    double course;
-    bool valid;
-    GprmcData() : latitude(0.0), longitude(0.0), speed(0.0), course(0.0), valid(false) {}
-};
-
-}  // namespace fixposition
-#endif  //__FIXPOSITION_DRIVER_LIB_MSG_DATA__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp
deleted file mode 100644
index cb57b5a..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nmea_type.hpp
+++ /dev/null
@@ -1,567 +0,0 @@
-/**
- *  @file
- *  @brief Declaration of NMEA type messages
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE__
-#define __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE__
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/msg_data.hpp>
-#include <fixposition_driver_lib/time_conversions.hpp>
-
-namespace fixposition {
-
-// ------------ NMEA-GP-GGA ------------
-
-struct GP_GGA {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    std::string time_str;
-    Eigen::Vector3d llh;
-    char lat_ns;
-    char lon_ew;
-    char alt_unit;
-    int quality;
-    int num_sv;
-    float hdop;
-    float diff_age;
-    std::string diff_sta;
-    std::string sentence;
-
-    // Message structure
-    static constexpr char frame_id[] = "LLH";
-    static constexpr char child_frame_id[] = "FP_POI";
-    static constexpr char header_[] = "GPGGA";
-    static constexpr unsigned int kSize_ = 15;
-
-    GP_GGA() {
-        time_str = "";
-        llh.setZero();
-        lat_ns = ' ';
-        lon_ew = ' ';
-        alt_unit = ' ';
-        quality = -1;
-        num_sv = -1;
-        hdop = -1.0;
-        diff_age = -1.0;
-        diff_sta = "";
-        sentence = "";
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        time_str = "";
-        llh.setZero();
-        lat_ns = ' ';
-        lon_ew = ' ';
-        alt_unit = ' ';
-        quality = -1;
-        num_sv = -1;
-        hdop = -1.0;
-        diff_age = -1.0;
-        diff_sta = "";
-        sentence = "";
-    }
-};
-
-// ------------ NMEA-GP-GLL ------------
-
-struct GP_GLL {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    std::string time_str;
-    Eigen::Vector2d latlon;
-    char lat_ns;
-    char lon_ew;
-    char status;
-    char mode;
-
-    // Message structure
-    const std::string frame_id = "LLH";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "GPGLL";
-    static constexpr unsigned int kSize_ = 8;
-
-    GP_GLL() {
-        time_str = "";
-        latlon.setZero();
-        lat_ns = ' ';
-        lon_ew = ' ';
-        status = ' ';
-        mode = ' ';
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        time_str = "";
-        latlon.setZero();
-        lat_ns = ' ';
-        lon_ew = ' ';
-        status = ' ';
-        mode = ' ';
-    }
-};
-
-// ------------ NMEA-GN-GSA ------------
-
-struct GN_GSA {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    char mode_op;
-    int8_t mode_nav;
-    std::vector<int> ids;
-    float pdop;
-    float hdop;
-    float vdop;
-    int8_t gnss_id;
-
-    // Message structure
-    const std::string frame_id = "LLH";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "GNGSA";
-    static constexpr unsigned int kSize_ = 19;
-
-    GN_GSA() {
-        mode_op = ' ';
-        mode_nav = -1;
-        ids.clear();
-        pdop = -1.0;
-        hdop = -1.0;
-        vdop = -1.0;
-        gnss_id = -1;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        mode_op = ' ';
-        mode_nav = -1;
-        ids.clear();
-        pdop = -1.0;
-        hdop = -1.0;
-        vdop = -1.0;
-        gnss_id = -1;
-    }
-};
-
-// ------------ NMEA-GP-GST ------------
-
-struct GP_GST {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    std::string time_str;
-    float rms_range;
-    float std_major;
-    float std_minor;
-    float angle_major;
-    float std_lat;
-    float std_lon;
-    float std_alt;
-
-    // Message structure
-    const std::string frame_id = "LLH";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "GPGST";
-    static constexpr unsigned int kSize_ = 9;
-
-    GP_GST() {
-        time_str = "";
-        rms_range = -1.0;
-        std_major = -1.0;
-        std_minor = -1.0;
-        angle_major = -1.0;
-        std_lat = -1.0;
-        std_lon = -1.0;
-        std_alt = -1.0;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        time_str = "";
-        rms_range = -1.0;
-        std_major = -1.0;
-        std_minor = -1.0;
-        angle_major = -1.0;
-        std_lat = -1.0;
-        std_lon = -1.0;
-        std_alt = -1.0;
-    }
-};
-
-// ------------ NMEA-GX-GSV ------------
-
-struct GX_GSV {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    uint8_t sentences;
-    uint8_t sent_num;
-    uint8_t num_sats;
-    std::vector<unsigned int> sat_id;
-    std::vector<unsigned int> elev;
-    std::vector<unsigned int> azim;
-    std::vector<unsigned int> cno;
-    std::string signal_id;
-    SignalType type;
-
-    // Message structure
-    const std::string frame_id = "LLH";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "GXGST";
-    unsigned int kSize_ = 4;  // Maximum size: 4 + num_sats * 4
-
-    SignalType string2enum(const std::string& name) {
-        if (name == "GP") return SignalType::GPS;     // GPS
-        if (name == "GA") return SignalType::Galileo; // Galileo
-        if (name == "GB") return SignalType::BeiDou;  // BeiDou
-        if (name == "GL") return SignalType::GLONASS; // GLONASS
-        return SignalType::Invalid;
-    }
-
-    GX_GSV() {
-        sentences = 0;
-        sent_num = 0;
-        num_sats = 0;
-        sat_id.clear();
-        elev.clear();
-        azim.clear();
-        cno.clear();
-        type = SignalType::Invalid;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        sentences = 0;
-        sent_num = 0;
-        num_sats = 0;
-        sat_id.clear();
-        elev.clear();
-        azim.clear();
-        cno.clear();
-        type = SignalType::Invalid;
-    }
-};
-
-// ------------ NMEA-GP-HDT ------------
-
-struct GP_HDT {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    float heading;
-    char true_ind;
-
-    // Message structure
-    const std::string frame_id = "LLH";
-    const std::string child_frame_id = "FP_POI";
-    const std::string header_ = "GPHDT";
-    static constexpr unsigned int kSize_ = 3;
-
-    GP_HDT() {
-        heading = 0.0;
-        true_ind = ' ';
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        heading = 0.0;
-        true_ind = ' ';
-    }
-};
-
-// ------------ NMEA-GP-RMC ------------
-
-struct GP_RMC {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    std::string date_str;
-    std::string time_str;
-    char status;
-    Eigen::Vector2d latlon;
-    char lat_ns;
-    char lon_ew;
-    float speed;
-    float speed_ms;
-    float course;
-    char mode;
-
-    // Message structure
-    static constexpr char frame_id[] = "LLH";
-    static constexpr char child_frame_id[] = "FP_POI";
-    static constexpr char header_[] = "GPRMC";
-    static constexpr unsigned int kSize_ = 13;
-
-    GP_RMC() {
-        date_str = "";
-        time_str = "";
-        status = ' ';
-        latlon.setZero();
-        lat_ns = ' ';
-        lon_ew = ' ';
-        speed = 0.0;
-        speed_ms = 0.0;
-        course = 0.0;
-        mode = ' ';
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        date_str = "";
-        time_str = "";
-        status = ' ';
-        latlon.setZero();
-        lat_ns = ' ';
-        lon_ew = ' ';
-        speed = 0.0;
-        speed_ms = 0.0;
-        course = 0.0;
-        mode = ' ';
-    }
-};
-
-// ------------ NMEA-GP-VTG ------------
-
-struct GP_VTG {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    float cog_true;
-    char cog_ref_t;
-    float cog_mag;
-    char cog_ref_m;
-    float sog_knot;
-    char sog_unit_n;
-    float sog_kph;
-    char sog_unit_k;
-    char mode;
-
-    // Message structure
-    static constexpr char frame_id[] = "LLH";
-    static constexpr char child_frame_id[] = "FP_POI";
-    static constexpr char header_[] = "GPVTG";
-    static constexpr unsigned int kSize_ = 10;
-
-    GP_VTG() {
-        cog_true = 0.0;
-        cog_ref_t = ' ';
-        cog_mag = 0.0;
-        cog_ref_m = ' ';
-        sog_knot = 0.0;
-        sog_unit_n = ' ';
-        sog_kph = 0.0;
-        sog_unit_k = ' ';
-        mode = ' ';
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        cog_true = 0.0;
-        cog_ref_t = ' ';
-        cog_mag = 0.0;
-        cog_ref_m = ' ';
-        sog_knot = 0.0;
-        sog_unit_n = ' ';
-        sog_kph = 0.0;
-        sog_unit_k = ' ';
-        mode = ' ';
-    }
-};
-
-// ------------ NMEA-GP-ZDA ------------
-
-struct GP_ZDA {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    std::string date_str;  // Format: dd/mm/yyyy
-    std::string time_str;  // Format: hhmmss.ss(ss)
-    times::GpsTime stamp;
-    uint8_t local_hr;
-    uint8_t local_min;
-
-    // Message structure
-    static constexpr char frame_id[] = "LLH";
-    static constexpr char child_frame_id[] = "FP_POI";
-    static constexpr char header_[] = "GPZDA";
-    static constexpr unsigned int kSize_ = 7;
-
-    GP_ZDA() {
-        stamp = fixposition::times::GpsTime();
-        time_str = "";
-        date_str = "";
-        local_hr = 0;
-        local_min = 0;
-    }
-
-    void ConvertFromTokens(const std::vector<std::string>& tokens);
-
-    void ResetData() {
-        stamp = fixposition::times::GpsTime();
-        time_str = "";
-        date_str = "";
-        local_hr = 0;
-        local_min = 0;
-    }
-};
-
-// ------------ NmeaMessage ------------
-struct GnssSignalStats {
-    unsigned int azim;
-    unsigned int elev;
-    unsigned int cno_l1;
-    unsigned int cno_l2;
-
-    GnssSignalStats() {
-        azim = 0;
-        elev = 0;
-        cno_l1 = 0;
-        cno_l2 = 0;
-    }
-
-    void ResetData() {
-        azim = 0;
-        elev = 0;
-        cno_l1 = 0;
-        cno_l2 = 0;
-    }
-};
-
-struct NmeaMessage {
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    // Message fields
-    std::string gpgga_time_str;      // GP_GGA
-    std::string gpzda_time_str;      // GP_ZDA
-    std::string time_str;            // GP_ZDA (alt. GP_GGA, GP_GST, GP_RMC)
-    std::string date_str;            // GP_ZDA (alt. GP_RMC)
-    times::GpsTime stamp;            // GP_ZDA
-    Eigen::Vector3d llh;             // GP_GGA (alt. LL only for GP_GLL, GP_RMC)
-    uint8_t quality;                 // GP_GGA (alt. GP_RMC, GP_VTG, or limited GP_GLL, GP_GSA)
-    uint8_t num_sv;                  // GP_GGA
-    std::vector<int> ids;            // GN_GSA
-    float hdop_receiver;             // GP_GGA
-    float pdop;                      // GN_GSA
-    float hdop;                      // GN_GSA (alt. GP_GGA)
-    float vdop;                      // GN_GSA
-    float rms_range;                 // GP_GST
-    float std_major;                 // GP_GST
-    float std_minor;                 // GP_GST
-    float angle_major;               // GP_GST
-    float std_lat;                   // GP_GST (alt. GP_GGA)
-    float std_lon;                   // GP_GST (alt. GP_GGA)
-    float std_alt;                   // GP_GST (alt. GP_GGA)
-    Eigen::Matrix<double, 3, 3> cov; // GP_GST (alt. GP_GGA)
-    uint8_t cov_type;                // GP_GST (alt. GP_GGA)
-    float heading;                   // GP_HDT
-    float speed;                     // GP_RMC (alt. GP_VTG)
-    float course;                    // GP_RMC (alt. GP_VTG)
-    float diff_age;                  // GP_GGA
-    std::string diff_sta;            // GP_GGA
-    std::unordered_map<SignalType, std::map<unsigned int, GnssSignalStats>> gnss_signals; // GX_GSV
-
-    /**
-     * @brief Check if GNSS epoch is complete
-     */
-    bool checkEpoch() {
-        if (!gpzda_time_str.empty() && (gpgga_time_str.compare(gpzda_time_str) == 0)) {
-            return true;
-        } else {
-            return false;
-        }
-    }
-
-    NmeaMessage() {
-        gpgga_time_str = "";
-        gpzda_time_str = "";
-        time_str = "";
-        date_str = "";
-        stamp = fixposition::times::GpsTime();
-        llh.setZero();
-        quality = -1;
-        diff_age = 0.0;
-        diff_sta = "";
-        ids.clear();
-        hdop_receiver = 0.0;
-        pdop = 0.0;
-        hdop = 0.0;
-        vdop = 0.0;
-        rms_range = 0.0;
-        std_major = 0.0;
-        std_minor = 0.0;
-        angle_major = 0.0;
-        std_lat = 0.0;
-        std_lon = 0.0;
-        std_alt = 0.0;
-        cov.setZero();
-        cov_type = 0;
-        num_sv = 0;
-        heading = 0.0;
-        speed = 0.0;
-        course = 0.0;
-    }
-
-    void ResetData() {
-        gpgga_time_str = "";
-        gpzda_time_str = "";
-        time_str = "";
-        date_str = "";
-        stamp = fixposition::times::GpsTime();
-        llh.setZero();
-        quality = -1;
-        diff_age = 0.0;
-        diff_sta = "";
-        ids.clear();
-        hdop_receiver = 0.0;
-        pdop = 0.0;
-        hdop = 0.0;
-        vdop = 0.0;
-        rms_range = 0.0;
-        std_major = 0.0;
-        std_minor = 0.0;
-        angle_major = 0.0;
-        std_lat = 0.0;
-        std_lon = 0.0;
-        std_alt = 0.0;
-        cov.setZero();
-        cov_type = 0;
-        num_sv = 0;
-        heading = 0.0;
-        speed = 0.0;
-        course = 0.0;
-    }
-
-    void AddNmeaEpoch(const GP_GGA& msg);
-    void AddNmeaEpoch(const GP_GLL& msg);
-    void AddNmeaEpoch(const GN_GSA& msg);
-    void AddNmeaEpoch(const GP_GST& msg);
-    void AddNmeaEpoch(const GX_GSV& msg);
-    void AddNmeaEpoch(const GP_HDT& msg);
-    void AddNmeaEpoch(const GP_RMC& msg);
-    void AddNmeaEpoch(const GP_VTG& msg);
-    void AddNmeaEpoch(const GP_ZDA& msg);
-};
-
-}  // namespace fixposition
-#endif  // __FIXPOSITION_DRIVER_LIB_CONVERTER_NMEA_TYPE__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp
deleted file mode 100644
index 7433c48..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/messages/nov_type.hpp
+++ /dev/null
@@ -1,711 +0,0 @@
-/*!
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Copyright (c) Fixposition AG
- *   /  /\  \   All right reserved
- *  /__/  \__\
- *
- *  Portions copyright NovAtel:
- *
- *     Copyright (c) 2020 NovAtel Inc.
- *
- *     Permission is hereby granted, free of charge, to any person obtaining a copy
- *     of this software and associated documentation files (the "Software"), to deal
- *     in the Software without restriction, including without limitation the rights
- *     to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- *     copies of the Software, and to permit persons to whom the Software is
- *     furnished to do so, subject to the following conditions:
- *
- *     The above copyright notice and this permission notice shall be included in all
- *     copies or substantial portions of the Software.
- *
- *     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- *     IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- *     FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- *     AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- *     LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- *     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- *     SOFTWARE.
- * \endverbatim
- *
- *   @file
- *   @brief NovAtel types and utilities
- */
-
-/* ****************************************************************************************************************** */
-#ifndef __FIXPOSITION_DRIVER_LIB_NOV_TYPE__
-#define __FIXPOSITION_DRIVER_LIB_NOV_TYPE__
-
-// Fundamental types
-#include <stdbool.h>
-#include <stddef.h>
-#include <stdint.h>
-
-/* ****************************************************************************************************************** */
-
-namespace fixposition {
-
-/**
- * @brief CRC32 calculation
- *
- * @param[in] data
- * @param[in] size
- * @return uint32_t
- */
-inline uint32_t nov_crc32(const uint8_t* data, const int size) {
-    uint32_t crc = 0;
-    for (int i = 0; i < size; i++) {
-        crc ^= data[i];
-        for (int j = 0; j < 8; j++) {
-            if (crc & 1) {
-                crc = (crc >> 1) ^ 0xedb88320u;
-            } else {
-                crc >>= 1;
-            }
-        }
-    }
-    return crc;
-}
-
-static constexpr uint8_t SYNC_CHAR_1 = 0xaa;
-static constexpr uint8_t SYNC_CHAR_2 = 0x44;
-static constexpr uint8_t SYNC_CHAR_3_LONG = 0x12;
-static constexpr uint8_t SYNC_CHAR_3_SHORT = 0x13;
-
-/* ****************************************************************************************************************** */
-/**
- * @name Novatel oem7 message flag definitions
- * @{
- */
-enum class MessageId : uint16_t {
-    GPGGA = 218,
-    RAWIMU = 268,
-    INSPVA = 507,
-    HEADING2 = 1335,
-    BESTPOS = 42,
-    BERSTXYZ = 241,
-    BESTGNSSPOS = 1429,
-    INSPVAX = 1465,
-    BESTXYZ = 241,
-    BESTUTM = 726,
-    BESTVEL = 99,
-    CORRIMUS = 2264,
-    IMURATECORRIMUS = 1362,
-    INSCONFIG = 1945,
-    INSPVAS = 508,
-    INSSTDEV = 2051,
-    PSRDOP2 = 1163,
-    RXSTATUS = 93,
-    TIME = 101
-};
-
-/**
- * @brief See https://docs.novatel.com/OEM7/Content/SPAN_Logs/INSATTX.htm#ExtendedSolutionStatus
- * Bitwise Masks for all 32 bits. For usage please refer to INSPVAX converter.
- */
-enum class ExtendedSolutionStatusIns : uint32_t {
-    POSITION_UPDATE = 0x00000001,
-    PHASE_UPDATE = 0x00000002,
-    ZERO_VEOLICYT_UPDATE = 0x00000004,
-    WHEEL_SENSOR_UPDATE = 0x00000008,
-    HEADING_UPDATE = 0x00000010,
-    EXTERNAL_POSITION_UPDATE = 0x00000020,
-    INS_SOLUTION_CONVERGENCE = 0x00000040,
-    DOPPLER_UPDATE = 0x00000080,
-    PSEUDORANGE_UPDATE = 0x00000100,
-    VELOCITY_UPDATE = 0x00000200,
-    // RESERVED = 0x00000400,
-    DR_UPDATE = 0x000000800,
-    PHASE_WINDUP_UPDATE = 0x00001000,
-    COURSE_OVER_GROUND_UPDATE = 0x00002000,
-    EXTERNAL_VELOCITY_UPDATE = 0x00004000,
-    EXTERNAL_ATTITUDE_UPDATE = 0x00008000,
-
-    EXTERNAL_HEADING_UPDATE = 0x00010000,
-    EXTERNAL_HEIGHT_UPDATE = 0x00020000,
-    // RESERVED = 0x00040000,
-    // RESERVED = 0x00080000,
-
-    // RESERVED = 0x00100000,
-    // RESERVED = 0x00200000,
-    // RESERVED = 0x00400000,
-    // RESERVED = 0x00800000,
-
-    TURN_ON_BIAS_ESTIMATED = 0x01000000,
-    ALIGNMENT_DIRECTION_VERIFIED = 0x02000000,
-    ALIGNMENT_INDICATION_1 = 0x04000000,
-    ALIGNMENT_INDICATION_2 = 0x08000000,
-
-    ALIGNMENT_INDICATION_3 = 0x10000000,
-    NVM_SEED_INDICATION_1 = 0x20000000,
-    NVM_SEED_INDICATION_2 = 0x40000000,
-    NVM_SEED_INDICATION_3 = 0x80000000,
-};
-
-/**
- * @brief See https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#GPS_GLONASSSignalUsedMask
- */
-enum class GpsGlonassSignalUsed : uint8_t {
-    GPS_L1 = 0x01,
-    GPS_L2 = 0x02,
-    GPS_L5 = 0x04,
-    GLONASS_L1 = 0x10,
-    GLONASS_L2 = 0x20,
-    GLONASS_L5 = 0x40,
-};
-
-/**
- * @brief See https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#Galileo_BeiDouSignalUsedMask
- */
-enum class GalileoBeidouSignalUsed : uint8_t {
-    GALILEO_L1 = 0x01,
-    GALILEO_L2 = 0x02,
-    GALILEO_L5 = 0x04,
-    BEIDOU_L1 = 0x10,
-    BEIDOU_L2 = 0x20,
-    BEIDOU_L5 = 0x40,
-};
-
-/**
- * @brief See https://docs.novatel.com/OEM7/Content/SPAN_Logs/INSATT.htm#InertialSolutionStatus
- */
-enum class InertialSolutionStatus : uint32_t {
-    INS_INACTIVE = 0,
-    INS_ALIGNING = 1,
-    INS_HIGH_VARIANCE = 2,
-    INS_SOLUTION_GOOD = 3,
-    INS_SOLUTION_FREE = 6,
-    INS_ALIGNMENT_COMPLETE = 7,
-    DETERMINING_ORIENTATION = 8,
-    WAITING_INITIAL_POS = 9,
-    WAITING_AZIMUTH = 10,
-    INITIALIZING_BIASES = 11,
-    MOTION_DETECT = 12
-};
-
-/**
- * @brief See https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#Position_VelocityType
- */
-enum class PositionOrVelocityType : uint32_t {
-    NONE = 0,
-    FIXEDPOS = 1,
-    FIXEDHEIGHT = 2,
-    DOPPLER_VELOCITY = 8,
-    SINGLE = 16,
-    PSRDIFF = 17,
-    WAAS = 18,
-    PROPAGATED = 19,
-    L1_FLOAT = 32,
-    NARROW_FLOAT = 34,
-    L1_INT = 48,
-    WIDE_INT = 49,
-    NARROW_INT = 50,
-    RTK_DIRECT_INS = 51,
-    INS_SBAS = 52,
-    INS_PSRSP = 53,
-    INS_PSRDIFF = 54,
-    INS_RTKFLOAT = 55,
-    INS_RTKFIXED = 56,
-    PPP_CONVERGING = 68,
-    PPP = 69,
-    OPERATIONAL = 70,
-    WARNING = 71,
-    OUT_OF_BOUNDS = 72,
-    INS_PPP_CONVERGING = 73,
-    INS_PPP = 74,
-    PPP_BASIC_CONVERGING = 77,
-    PPP_BASIC = 78,
-    INS_PPP_BASIC_CONVERGING = 79,
-    INS_PPP_BASIC = 80
-};
-
-/**
- * @brief See https://docs.novatel.com/OEM7/Content/Messages/GPS_Reference_Time_Statu.htm#Table_GPSReferenceTimeStatus
- */
-enum class GpsReferenceTimeStatus : uint8_t {
-    UNKNOWN = 20,
-    APPROXIMATE = 60,
-    COARSEADJUSTING = 80,
-    COARSE = 100,
-    COARSESTEERING = 120,
-    FREEWHEELING = 130,
-    FINEADJUSTING = 140,
-    FINE = 160,
-    FINEBACKUPSTEERING = 170,
-    FINESTEERING = 180,  // I think we mainly use this one
-    SATTIME = 200
-};
-
-/**
- * @brief Stringify time status
- * @param[in]  time_status  The time status
- * @returns a unique string for the time status
- */
-const char* GpsReferenceTimeStatusStr(const GpsReferenceTimeStatus time_status);
-
-enum class MessageTypeSource : uint8_t {  // clang-format off
-    PRIMARY   = 0b00000000, //!< Primary antenna
-    SECONDARY = 0b00000001, //!< Secondary antenna
-    _MASK     = 0b00011111, //!< Mask for the source part of the message_type field
-};  // clang-format on
-
-/**
- * @brief See
- * https://docs.novatel.com/OEM7/Content/Messages/Binary.htm?tocpath=Commands%20%2526%20Logs%7CMessages%7C_____3#Table_BinaryMessageHeaderStructure
- */
-enum class MessageTypeFormat : uint8_t {  // clang-format off
-    BINARY      = 0b00000000, //!< Binary
-    ASCII       = 0b00100000, //!< ASCII
-    AASCII_NMEA = 0b01000000, //!< Abbreviated ASCII, NMEA
-    RESERVED    = 0b01100000, //!< Reserved
-    _MASK       = 0b01100000, //!< Mask for the format part of the message_type field
-};  // clang-format on
-
-enum class MessageTypeResponse : uint8_t {  // clang-format off
-    ORIGINAL = 0b00000000,
-    RESPONSE = 0b10000000,
-    _MASK    = 0b10000000, //!< Mask for the response part of the message_type field
-};  // clang-format on
-
-enum class PortAddress : uint8_t {  // clang-format off
-    NO_PORTS  = 0x00, //!< No ports specified
-    ALL_PORTS = 0x80, //!< All virtual ports for all ports
-    THISPORT  = 0xc0, //!< Current COM port
-    // there are many more...
-};  // clang-format on
-
-// https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#SolutionStatus
-enum class SolStat : uint32_t {  // clang-format off
-
-    SOL_COMPUTED     =  0, //!< Solution computed
-    INSUFFICIENT_OBS =  1, //!< Insufficient observations
-    NO_CONVERGENCE   =  2, //!< No convergence
-    SINGULARITY      =  3, //!< Singularity at parameters matrix
-    COV_TRACE        =  4, //!< Covariance trace exceeds maximum (trace > 1000 m)
-    TEST_DIST        =  5, //!< Test distance exceeded (maximum of 3 rejections if distance >10 km)
-    COLD_START       =  6, //!< Not yet converged from cold start
-    V_H_LIMIT        =  7, //!< Height or velocity limits exceeded (in accordance with export licensing restrictions)
-    VARIANCE         =  8, //!< Variance exceeds limits
-    RESIDUALS        =  9, //!< Residuals are too large
-    // there are some more...
-};  // clang-format on
-
-enum class DatumId : uint32_t {
-    WGS84 = 61,
-    USER = 63,
-};
-
-// https://docs.novatel.com/OEM7/Content/Logs/HEADING2.htm#SolutionSource
-enum class SolutionSource : uint8_t {  // clang-format off
-    PRIMARY   = 0b00000100, //!< Primary antenna
-    SECONDARY = 0b00001000, //!< Secondary antenna
-    _MASK     = 0b00001100, //!< Mask
-};  // clang-format on
-
-// https://docs.novatel.com/OEM7/Content/Logs/BESTPOS.htm#ExtendedSolutionStatus
-enum class ExtendedSolutionStatusGnss : uint8_t {  // clang-format off
-    SOL_VERIFIED = 0b00000001, //!< Solution verified
-    // There are more...
-};  // clang-format on
-
-/**
- * @}
- */
-/* ****************************************************************************************************************** */
-/**
- * @name Binary format definitions for Oem7 messages
- *
- * All Oem7 messages are 4-byte aligned, allowing simple casting into structs
- * @{
- */
-
-typedef uint32_t oem7_enum_t;
-typedef uint32_t oem7_bool_t;
-typedef uint8_t oem7_hex_t;
-typedef char oem7_char_t;
-
-static_assert(sizeof(oem7_char_t) == 1, "");
-static_assert(sizeof(double) == 8, "");
-static_assert(sizeof(float) == 4, "");
-
-struct __attribute__((packed)) Oem7MessageCommonHeaderMem {
-    char sync1;
-    char sync2;
-    char sync3;
-
-    uint8_t message_length;
-    uint16_t message_id;
-};
-
-struct __attribute__((packed)) Oem7MessageHeaderMem {
-    uint8_t sync1;
-    uint8_t sync2;
-    uint8_t sync3;
-
-    uint8_t header_length;
-    uint16_t message_id;
-    uint8_t message_type;
-    uint8_t port_address;
-    uint16_t message_length;
-    uint16_t sequence;
-    uint8_t idle_time;
-    uint8_t time_status;
-    uint16_t gps_week;
-    int32_t gps_milliseconds;
-    uint32_t receiver_status;
-    uint16_t reserved;
-    uint16_t receiver_version;
-};
-
-struct __attribute__((packed)) Oem7MessgeShortHeaderMem {
-    uint8_t sync1;
-    uint8_t sync2;
-    uint8_t sync3;
-
-    uint8_t message_length;
-    uint16_t message_id;
-    uint16_t gps_week;
-    int32_t gps_milliseconds;
-};
-
-struct __attribute__((packed)) BESTPOSMem {
-    oem7_enum_t sol_stat;
-    oem7_enum_t pos_type;
-    double lat;
-    double lon;
-    double hgt;
-    float undulation;
-    oem7_enum_t datum_id;
-    float lat_stdev;
-    float lon_stdev;
-    float hgt_stdev;
-    oem7_char_t stn_id[4];
-    float diff_age;
-    float sol_age;
-    uint8_t num_svs;
-    uint8_t num_sol_svs;
-    uint8_t num_sol_l1_svs;
-    uint8_t num_sol_multi_svs;
-    oem7_hex_t reserved;
-    oem7_hex_t ext_sol_stat;
-    uint8_t galileo_beidou_sig_mask;
-    uint8_t gps_glonass_sig_mask;
-};
-static_assert(sizeof(BESTPOSMem) == 72, "");
-// Could be changed: include more fields to become exactly same as pos, include short header or long while reading
-struct __attribute__((packed)) BESTXYZMem {
-    oem7_enum_t p_sol_stat;
-    oem7_enum_t pos_type;
-    double p_x;
-    double p_y;
-    double p_z;
-    float p_x_stdev;
-    float p_y_stdev;
-    float p_z_stdev;
-    oem7_enum_t v_sol_stat;
-    oem7_enum_t vel_type;
-    double v_x;
-    double v_y;
-    double v_z;
-    float v_x_stdev;
-    float v_y_stdev;
-    float v_z_stdev;
-};
-static_assert(sizeof(BESTXYZMem) == 88, "");
-
-struct __attribute__((packed)) BESTVELMem {
-    uint32_t sol_stat;
-    uint32_t vel_type;
-    float latency;
-    float diff_age;
-    double hor_speed;
-    double track_gnd;
-    double ver_speed;
-    float reserved;
-};
-static_assert(sizeof(BESTVELMem) == 44, "");
-
-struct __attribute__((packed)) INSPVASmem {
-    uint32_t gnss_week;
-    double seconds;
-    double latitude;
-    double longitude;
-    double height;
-    double north_velocity;
-    double east_velocity;
-    double up_velocity;
-    double roll;
-    double pitch;
-    double azimuth;
-    oem7_enum_t status;
-};
-static_assert(sizeof(INSPVASmem) == 88, "");
-
-struct __attribute__((packed)) CORRIMUSMem {
-    uint32_t imu_data_count;
-    double pitch_rate;
-    double roll_rate;
-    double yaw_rate;
-    double lateral_acc;
-    double longitudinal_acc;
-    double vertical_acc;
-    uint32_t reserved1;
-    uint32_t reserved2;
-};
-static_assert(sizeof(CORRIMUSMem) == 60, "");
-
-struct __attribute__((packed)) IMURATECORRIMUSMem {
-    uint32_t week;
-    double seconds;
-    double pitch_rate;
-    double roll_rate;
-    double yaw_rate;
-    double lateral_acc;
-    double longitudinal_acc;
-    double vertical_acc;
-};
-static_assert(sizeof(IMURATECORRIMUSMem) == 60, "");
-
-struct __attribute__((packed)) INSSTDEVMem {
-    float latitude_stdev;
-    float longitude_stdev;
-    float height_stdev;
-    float north_velocity_stdev;
-    float east_velocity_stdev;
-    float up_velocity_stdev;
-    float roll_stdev;
-    float pitch_stdev;
-    float azimuth_stdev;
-    uint32_t ext_sol_status;
-    uint16_t time_since_last_update;
-    uint16_t reserved1;
-    uint32_t reserved2;
-    uint32_t reserved3;
-};
-static_assert(sizeof(INSSTDEVMem) == 52, "");
-
-struct __attribute__((packed)) INSCONFIG_FixedMem {
-    oem7_enum_t imu_type;
-    uint8_t mapping;
-    uint8_t initial_alignment_velocity;
-    uint16_t heave_window;
-    oem7_enum_t profile;
-    oem7_hex_t enabled_updates[4];
-    oem7_enum_t alignment_mode;
-    oem7_enum_t relative_ins_output_frame;
-    oem7_bool_t relative_ins_output_direction;
-    oem7_hex_t ins_receiver_status[4];
-    uint8_t ins_seed_enabled;
-    uint8_t ins_seed_validation;
-    uint16_t reserved_1;
-    uint32_t reserved_2;
-    uint32_t reserved_3;
-    uint32_t reserved_4;
-    uint32_t reserved_5;
-    uint32_t reserved_6;
-    uint32_t reserved_7;
-};
-static_assert(sizeof(INSCONFIG_FixedMem) == 60, "");
-
-struct __attribute__((packed)) INSCONFIG_TranslationMem {
-    uint32_t translation;
-    uint32_t frame;
-    float x_offset;
-    float y_offset;
-    float z_offset;
-    float x_uncertainty;
-    float y_uncertainty;
-    float z_uncertainty;
-    uint32_t translation_source;
-};
-
-struct __attribute__((packed)) INSCONFIG_RotationMem {
-    uint32_t rotation;
-    uint32_t frame;
-    float x_rotation;
-    float y_rotation;
-    float z_rotation;
-    float x_rotation_stdev;
-    float y_rotation_stdev;
-    float z_rotation_stdev;
-    uint32_t rotation_source;
-};
-
-struct __attribute__((packed)) INSPVAXMem {
-    oem7_enum_t ins_status;
-    oem7_enum_t pos_type;
-    double latitude;
-    double longitude;
-    double height;
-    float undulation;
-    double north_velocity;
-    double east_velocity;
-    double up_velocity;
-    double roll;
-    double pitch;
-    double azimuth;
-    float latitude_stdev;
-    float longitude_stdev;
-    float height_stdev;
-    float north_velocity_stdev;
-    float east_velocity_stdev;
-    float up_velocity_stdev;
-    float roll_stdev;
-    float pitch_stdev;
-    float azimuth_stdev;
-    uint32_t extended_status;
-    uint16_t time_since_update;
-};
-static_assert(sizeof(INSPVAXMem) == 126, "");
-
-struct __attribute__((packed)) HEADING2Mem {
-    oem7_enum_t sol_status;
-    oem7_enum_t pos_type;
-    float length;
-    float heading;
-    float pitch;
-    float reserved;
-    float heading_stdev;
-    float pitch_stdev;
-    oem7_char_t rover_stn_id[4];
-    oem7_char_t master_stn_id[4];
-    uint8_t num_sv_tracked;
-    uint8_t num_sv_in_sol;
-    uint8_t num_sv_obs;
-    uint8_t num_sv_multi;
-    uint8_t sol_source;
-    uint8_t ext_sol_status;
-    uint8_t galileo_beidou_sig_mask;
-    uint8_t gps_glonass_sig_mask;
-};
-static_assert(sizeof(HEADING2Mem) == 48, "");
-
-struct __attribute__((packed)) BESTUTMMem {
-    oem7_enum_t sol_stat;
-    oem7_enum_t pos_type;
-    uint32_t lon_zone_number;
-    uint32_t lat_zone_letter;
-    double northing;
-    double easting;
-    double height;
-    float undulation;
-    uint32_t datum_id;
-    float northing_stddev;
-    float easting_stddev;
-    float height_stddev;
-    char stn_id[4];
-    float diff_age;
-    float sol_age;
-    uint8_t num_svs;
-    uint8_t num_sol_svs;
-    uint8_t num_sol_ggl1_svs;
-    uint8_t num_sol_multi_svs;
-    uint8_t reserved;
-    uint8_t ext_sol_stat;
-    uint8_t galileo_beidou_sig_mask;
-    uint8_t gps_glonass_sig_mask;
-};
-static_assert(sizeof(BESTUTMMem) == 80, "");
-
-struct __attribute__((packed)) RXSTATUSMem {
-    uint32_t error;
-    uint32_t num_status_codes;
-    uint32_t rxstat;
-    uint32_t rxstat_pri_mask;
-    uint32_t rxstat_set_mask;
-    uint32_t rxstat_clr_mask;
-    uint32_t aux1_stat;
-    uint32_t aux1_stat_pri;
-    uint32_t aux1_stat_set;
-    uint32_t aux1_stat_clr;
-    uint32_t aux2_stat;
-    uint32_t aux2_stat_pri;
-    uint32_t aux2_stat_set;
-    uint32_t aux2_stat_clr;
-    uint32_t aux3_stat;
-    uint32_t aux3_stat_pri;
-    uint32_t aux3_stat_set;
-    uint32_t aux3_stat_clr;
-    uint32_t aux4_stat;
-    uint32_t aux4_stat_pri;
-    uint32_t aux4_stat_set;
-    uint32_t aux4_stat_clr;
-};
-static_assert(sizeof(RXSTATUSMem) == 88, "");
-
-struct __attribute__((packed)) TIMEMem {
-    uint32_t clock_status;
-    double offset;
-    double offset_std;
-    double utc_offset;
-    uint32_t utc_year;
-    uint8_t utc_month;
-    uint8_t utc_day;
-    uint8_t utc_hour;
-    uint8_t utc_min;
-    uint32_t utc_msec;
-    uint32_t utc_status;
-};
-static_assert(sizeof(TIMEMem) == 44, "");
-
-struct __attribute__((packed)) PSRDOP2_FixedMem {
-    float gdop;
-    float pdop;
-    float hdop;
-    float vdop;
-};
-static_assert(sizeof(PSRDOP2_FixedMem) == 16, "");
-
-struct __attribute__((packed)) PSRDOP2_SystemMem {
-    uint32_t system;
-    float tdop;
-};
-
-struct __attribute__((packed)) RAWIMUMem {
-    uint32_t week;
-    double seconds;
-    uint32_t imu_stat;
-    int32_t z_accel;
-    int32_t y_accel;
-    int32_t x_accel;
-    int32_t z_gyro;
-    int32_t y_gyro;
-    int32_t x_gyro;
-};
-static_assert(sizeof(RAWIMUMem) == 40, "");
-
-/**
- * @brief See https://docs.novatel.com/OEM7/Content/SPAN_Logs/BESTGNSSPOS.htm
- */
-struct __attribute__((packed)) BESTGNSSPOSMem {
-    oem7_enum_t sol_stat;
-    oem7_enum_t pos_type;
-    double lat;
-    double lon;
-    double hgt;
-    float undulation;
-    oem7_enum_t datum_id;
-    float lat_stdev;
-    float lon_stdev;
-    float hgt_stdev;
-    oem7_char_t stn_id[4];
-    float diff_age;
-    float sol_age;
-    uint8_t num_svs;
-    uint8_t num_sol_svs;
-    uint8_t num_sol_l1_svs;
-    uint8_t num_sol_multi_svs;
-    oem7_hex_t reserved;
-    oem7_hex_t ext_sol_stat;
-    uint8_t galileo_beidou_sig_mask;
-    uint8_t gps_glonass_sig_mask;
-};
-static_assert(sizeof(BESTGNSSPOSMem) == 72, "");
-
-const size_t OEM7_BINARY_MSG_HDR_LEN = sizeof(Oem7MessageHeaderMem);
-const size_t OEM7_BINARY_MSG_SHORT_HDR_LEN = sizeof(Oem7MessgeShortHeaderMem);
-
-/* ****************************************************************************************************************** */
-}  // namespace fixposition
-#endif  // __LIB_NOVATEL_TYPE_H__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp
index 03f5b7d..8e8fd2b 100644
--- a/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp
+++ b/fixposition_driver_lib/include/fixposition_driver_lib/params.hpp
@@ -1,51 +1,63 @@
 /**
- *  @file
- *  @brief Parameters
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Parameters
  */
 
 #ifndef __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__
 #define __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__
 
-/* SYSTEM / STL */
+/* LIBC/STL */
 #include <string>
 #include <vector>
 
-namespace fixposition {
+/* EXTERNAL */
+#include <fpsdk_common/parser/fpa.hpp>
 
-enum class INPUT_TYPE { TCP = 1, SERIAL = 2 };
+/* PACKAGE */
 
-struct FpOutputParams {
-    int rate;                          //!< loop rate of the main read loop
-    double reconnect_delay;            //!< wait time in [s] until retry connection
-    INPUT_TYPE type;                   //!< TCP or SERIAL
-    std::vector<std::string> formats;  //!< data formats to convert, supports "FP" for now
-    std::string qos_type;              //!< ROS QoS type, supports "sensor_<short/long>" and "default_<short/long>"
-    bool cov_warning;                  //!< enable/disable covariance warning
-    bool nav2_mode;                    //!< enable/disable nav2 mode
+namespace fixposition {
+/* ****************************************************************************************************************** */
 
-    std::string ip;    //!< IP address for TCP connection
-    std::string port;  //!< Port for TCP connection
-    int baudrate;      //!< baudrate of serial connection
-};
-struct CustomerInputParams {
-    std::string speed_topic;  //!< Input ROS topic for Speed measurements
-    std::string rtcm_topic;   //!< Input ROS topic for RTCM3 messages
+/**
+ * @brief Parameters for the sensor driver
+ *
+ * See launch/config.yaml for documentation
+ */
+struct DriverParams {
+    std::string stream_;
+    double reconnect_delay_ = 5.0;
+    std::vector<std::string> messages_;
+    fpsdk::common::parser::fpa::FpaEpoch nmea_epoch_ = fpsdk::common::parser::fpa::FpaEpoch::UNSPECIFIED;
+    bool raw_output_ = false;
+    bool cov_warning_ = false;
+    bool nav2_mode_ = false;
+
+    // Check if entry is in messges_
+    bool MessageEnabled(const std::string& message_name) const;
 };
 
-struct FixpositionDriverParams {
-    FpOutputParams fp_output;
-    CustomerInputParams customer_input;
+/**
+ * @brief Parameters for the ROS nodes
+ *
+ * See launch/config.yaml for documentation
+ */
+struct NodeParams {
+    std::string output_ns_;
+    std::string speed_topic_;
+    std::string corr_topic_;
+    std::string qos_type_;
 };
 
-}  // namespace fixposition
+bool StrToEpoch(const std::string& str, fpsdk::common::parser::fpa::FpaEpoch& epoch);
 
-#endif
+/* ****************************************************************************************************************** */
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_LIB_PARAMS_HPP__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp
deleted file mode 100644
index 39aa118..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/parser.hpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/**
- *  @file
- *  @brief Parser functions
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_PARSER__
-#define __FIXPOSITION_DRIVER_LIB_PARSER__
-
-/* SYSTEM / STL */
-#include <string>
-#include <vector>
-
-namespace fixposition {
-
-/**
- * @brief Check If msg is NMEA
- *
- * @param[in] buf start pointer of a char* buffer
- * @param[in] size size of the buffer to check
- * @return int the length of the NMEA message found. If no NMEA found then 0. If size argument is too small then -1
- */
-int IsNmeaMessage(const char* buf, const int size);
-
-/**
- * @brief Check If msg is NOV_B
- *
- * @param[in] buf buffer ptr
- * @param[in] size size
- * @return int the length of the NOV_B message found. If no NOV_B found then 0. If size argument is too small then -1
- */
-int IsNovMessage(const uint8_t* buf, const int size);
-
-}  // namespace fixposition
-#endif  // __FIXPOSITION_DRIVER_LIB_HELPER__
diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp
deleted file mode 100644
index 57b20d2..0000000
--- a/fixposition_driver_lib/include/fixposition_driver_lib/time_conversions.hpp
+++ /dev/null
@@ -1,271 +0,0 @@
-/**
- *  @file
- *  @brief Declaration of time conversion functions
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-#ifndef __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__
-#define __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__
-
-/* EXTERNAL */
-#include <eigen3/Eigen/Core>
-#include <eigen3/Eigen/Geometry>
-#include <boost/date_time/posix_time/posix_time.hpp>
-
-namespace BOOST_POSIX = boost::posix_time;
-
-namespace fixposition {
-
-namespace times {
-/**
- * @brief Type for nanosecond times
- *
- */
-using TIME_NS_T = uint64_t;
-
-namespace Constants {
-
-static constexpr int gps_leap_time_s = 18;  /// gps leap seconds
-
-/// unix posix time begin
-static const BOOST_POSIX::ptime unix_epoch_begin = BOOST_POSIX::time_from_string("1970-01-01 00:00:00.000");
-/// gps time begin
-static const BOOST_POSIX::ptime gps_epoch_begin = BOOST_POSIX::time_from_string("1980-01-06 00:00:00.000");
-static constexpr int sec_per_week = 604800;  //!< seconds per week
-static constexpr int sec_per_day = 86400;    //!< seconds per day
-
-static constexpr double ns_to_sec = 1.0e-9;  //!< convert ns to seconds
-static constexpr double sec_to_ns = 1.0e9;   //!< convert seconds to ns
-
-}  // namespace Constants
-
-/**
- * @brief Time formatted as GPS Week Number wno and Time of Week tow. Only time in this format
- * is GPS-Time, all other time formats are UTC.
- *
- */
-class GpsTime {
-   public:
-    int wno;                             //!> week number
-    double tow;                          //!> time of week
-    static const int wno_precision = 4;  // 4 is enough because week number is around 2000;
-    static const int precision = 9;
-    static const int length = 10;
-
-    /**
-     * @brief Construct a new GpsTime object
-     *
-     */
-    GpsTime() : wno(0), tow(0.0) {}
-
-    /**
-     * @brief Construct a new GpsTime object
-     *
-     * @param[in] week wno
-     * @param[in] sec tow
-     */
-    GpsTime(int week, double sec) : wno(week), tow(sec) {
-        int delta_week = std::floor(tow / Constants::sec_per_week);
-        wno += delta_week;
-        tow = tow - delta_week * Constants::sec_per_week;
-    }
-
-    GpsTime(GpsTime const&) = default;
-
-    GpsTime& operator=(const GpsTime gws) {
-        tow = gws.tow;
-        wno = gws.wno;
-        return *this;
-    }
-
-    GpsTime& operator+=(const double sec) {
-        double gps_sec_tmp = tow + sec;
-        // std::cout << gps_sec_tmp << std::endl;
-        int delta_week = std::floor(gps_sec_tmp / Constants::sec_per_week);
-        // std::cout << delta_week << std::endl;
-        wno += delta_week;
-        tow = gps_sec_tmp - delta_week * Constants::sec_per_week;
-        return *this;
-    }
-
-    GpsTime& operator+=(const GpsTime in) {
-        double d_sec = in.wno * Constants::sec_per_week + in.tow;
-        *this += d_sec;
-        return *this;
-    }
-
-    GpsTime& operator-=(const double sec) {
-        *this += -sec;
-        return *this;
-    }
-
-    GpsTime& operator-=(const GpsTime in) {
-        double d_sec = in.wno * Constants::sec_per_week + in.tow;
-        *this -= d_sec;
-        return *this;
-    }
-
-    GpsTime operator+(double sec) {
-        GpsTime res(*this);
-        res += sec;
-        return res;
-    }
-
-    GpsTime operator+(const GpsTime in) {
-        GpsTime res(*this);
-        res += in;
-        return res;
-    }
-
-    GpsTime operator-(double sec) {
-        GpsTime res(*this);
-        res -= sec;
-        return res;
-    }
-
-    GpsTime operator-(const GpsTime in) {
-        GpsTime res(*this);
-        res -= in;
-        return res;
-    }
-
-    bool operator==(const GpsTime gws) { return std::abs(tow - gws.tow) < 1e-3 && wno == gws.wno; }
-
-    bool operator>(const GpsTime gws) {
-        if (wno == gws.wno) {
-            return tow > gws.tow;
-        } else {
-            return wno > gws.wno;
-        }
-    }
-
-    bool operator<(const GpsTime gws) {
-        if (wno == gws.wno) {
-            return tow < gws.tow;
-        } else {
-            return wno < gws.wno;
-        }
-    }
-
-    bool operator!=(const GpsTime gws) { return !(*this == gws); }
-
-    /**
-     * @brief Operator << for use in stream
-     * hard coded week size as 4 and second precision length according to class
-     * member
-     *
-     * @param[in] stream
-     * @param[in] gps_time
-     * @return std::ostream&
-     */
-    friend std::ostream& operator<<(std::ostream& stream, const GpsTime& gps_time) {
-        std::string week = std::to_string(gps_time.wno);
-        week.resize(4, ' ');
-        stream << week << " ";
-        std::stringstream sec_ss;
-        sec_ss.precision(gps_time.precision);
-        sec_ss << gps_time.tow;
-        std::string sec = sec_ss.str();
-        if (sec.find(".") == std::string::npos) {
-            sec = sec + ".";
-        }
-        sec.resize(gps_time.length, '0');
-        stream << sec;
-
-        return stream;
-    }
-};
-
-/**
- * @brief
- * Only work after 2017.1.1
- *
- * @param[in] gps_time
- * @return BOOST_POSIX::ptime
- */
-inline BOOST_POSIX::ptime GpsTimeToPtime(const GpsTime& gps_time) {
-    TIME_NS_T micro_s = (gps_time.wno * Constants::sec_per_week + gps_time.tow - Constants::gps_leap_time_s) * 1e6;
-    return Constants::gps_epoch_begin + BOOST_POSIX::microseconds(micro_s);
-}
-
-/**
- * @brief
- *  Only work after 2017.1.1
- *
- * @param[in] boost_ptime
- * @return GpsTime
- */
-inline GpsTime PtimeToGpsTime(const BOOST_POSIX::ptime& boost_ptime) {
-    BOOST_POSIX::time_duration gps_duration = boost_ptime - Constants::gps_epoch_begin;
-    int weekcount = gps_duration.total_seconds() / Constants::sec_per_week;
-    double sec_in_week =
-        gps_duration.total_microseconds() / 1e6 - weekcount * Constants::sec_per_week + Constants::gps_leap_time_s;
-    return GpsTime(weekcount, sec_in_week);
-}
-
-/**
- * @brief Convert UTC time with milliseconds to GPS time
- *
- * @param[in] utcTimeString
- * @param[in] gpsWeek
- * @param[in] gpsTimeOfWeek
- * @return GpsTime
- */
-inline void convertToGPSTime(const std::string& utcTimeString, std::string& gpsWeek, std::string& gpsTimeOfWeek) {
-    // Define constants
-    const double secondsInWeek = 604800.0; // 7 days in seconds
-
-    // Parse the input string
-    std::tm tmTime = {};
-    std::istringstream iss(utcTimeString);
-    iss >> std::get_time(&tmTime, "%d/%m/%Y %H:%M:%S");
-
-    // Read milliseconds from input
-    char dot;
-    std::string milliseconds;
-    iss >> dot >> milliseconds;
-    double ms = std::stod("0." + milliseconds);
-    
-    if (iss.fail()) {
-        std::cerr << "Error parsing input string.\n";
-        return;
-    }
-    
-    // Convert UTC time to time since epoch
-    std::time_t utcTime = std::mktime(&tmTime);
-
-    // GPS epoch time (January 6, 1980)
-    std::tm gpsEpoch = {};
-    gpsEpoch.tm_year = 80; // years since 1900
-    gpsEpoch.tm_mon = 0;   // months since January
-    gpsEpoch.tm_mday = 6;  // day of the month
-    std::time_t gpsEpochTime = std::mktime(&gpsEpoch);
-
-    // Calculate GPS time of week and GPS week number
-    double timeDifference = std::difftime(utcTime, gpsEpochTime);
-    int gpsWeekNumber = static_cast<int>(std::floor(timeDifference / secondsInWeek));
-    double gpsTime = std::fmod(timeDifference, secondsInWeek);
-
-    // Add milliseconds to GPS time
-    gpsTime += (Constants::gps_leap_time_s + ms);
-
-    // Convert results to strings
-    std::ostringstream ossWeek, ossTime;
-    ossWeek << gpsWeekNumber;
-    ossTime << std::fixed << std::setprecision(6) << gpsTime;
-
-    gpsWeek = ossWeek.str();
-    gpsTimeOfWeek = ossTime.str();
-}
-
-}  // namespace times
-}  // namespace fixposition
-#endif  // __FIXPOSITION_DRIVER_LIB_TIME_CONVERSIONS__
diff --git a/fixposition_driver_lib/package.xml b/fixposition_driver_lib/package.xml
index 09ca91a..1173f47 100644
--- a/fixposition_driver_lib/package.xml
+++ b/fixposition_driver_lib/package.xml
@@ -1,13 +1,10 @@
 <?xml version="1.0"?>
 <package format="3">
     <name>fixposition_driver_lib</name>
-    <version>5.0.0</version>
-    <description>Fixposition ROS Driver Lib</description>
-
-    <maintainer email="info@fixposition.com">Fixposition AG</maintainer>
-
+    <version>8.0.0</version>
+    <description>Fixposition ROS driver lib</description>
+    <maintainer email="support@fixposition.com">Fixposition Support</maintainer>
     <license>MIT</license>
-
     <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
     <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
     <test_depend condition="$ROS_VERSION == 2">ament_cmake_gtest</test_depend>
@@ -20,6 +17,10 @@
     <depend>geometry_msgs</depend>
     <depend condition="$ROS_VERSION == 1">message_generation</depend>
     <depend condition="$ROS_VERSION == 1">message_runtime</depend>
+    <depend>fpsdk_common</depend>
+    <depend condition="$ROS_VERSION == 1">fpsdk_ros1</depend>
+    <depend condition="$ROS_VERSION == 2">fpsdk_ros2</depend>
+
     <export>
         <build_type>cmake</build_type>
     </export>
diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp
index e41bf00..192cfb6 100644
--- a/fixposition_driver_lib/src/fixposition_driver.cpp
+++ b/fixposition_driver_lib/src/fixposition_driver.cpp
@@ -1,521 +1,551 @@
 /**
- *  @file
- *  @brief Implementation of FixpositionDriver class
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Fixposition sensor driver
  */
 
-/* SYSTEM / STL */
+/* LIBC/STL */
+#include <cstdint>
+#include <cstring>
+#include <functional>
+#include <stdexcept>
+
+/* EXTERNAL */
 #include <arpa/inet.h>
-#include <errno.h>
 #include <fcntl.h>
-#include <stdexcept>
 
-/* PACKAGE */
-#include <fixposition_driver_lib/fixposition_driver.hpp>
-#include <fixposition_driver_lib/parser.hpp>
+#include <fpsdk_common/logging.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/nmea.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_common/string.hpp>
 
-#ifndef B460800
-#define B460800 460800
-#endif
-#ifndef B500000
-#define B500000 500000
-#endif
-#ifndef B921600
-#define B921600 921600
-#endif
-#ifndef B1000000
-#define B1000000 1000000
-#endif
+/* PACKAGE */
+#include "fixposition_driver_lib/fixposition_driver.hpp"
 
 namespace fixposition {
-FixpositionDriver::FixpositionDriver(const FixpositionDriverParams& params) : params_(params) {
-    // connect to the sensor
-    if (!Connect()) {
-        if (params_.fp_output.type == INPUT_TYPE::TCP) {
-            throw std::runtime_error("Unable to connect to the sensor via TCP");
-        } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) {
-            throw std::runtime_error("Unable to connect to the sensor via Serial");
-        } else {
-            throw std::runtime_error("Unable to connect to the sensor, verify configuration");
-        }
-    }
+/* ****************************************************************************************************************** */
+
+using namespace fpsdk::common;
+using namespace fpsdk::common::parser;
+
+FixpositionDriver::FixpositionDriver(const DriverParams& params)
+    : /* clang-format off */
+    params_   { params },
+    worker_   { "driver", std::bind(&FixpositionDriver::Worker, this, std::placeholders::_1) }  // clang-format on
+{}
 
-    // initialize converters
-    if (!InitializeConverters()) {
-        throw std::runtime_error("Could not initialize output converter!");
+FixpositionDriver::~FixpositionDriver() { StopDriver(); }
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+bool FixpositionDriver::Connect() {
+    if (sensor_fd_ >= 0) {
+        WARNING("Already connected to sensor");
+        return true;
     }
-}
 
-FixpositionDriver::~FixpositionDriver() {
-    if (client_fd_ != -1) {
-        if (params_.fp_output.type == INPUT_TYPE::SERIAL) {
-            tcsetattr(client_fd_, TCSANOW, &options_save_);
+    if (string::StrStartsWith(params_.stream_, "tcpcli://")) {
+        const auto parts = string::StrSplit(params_.stream_.substr(9), ":");
+        int port = 0;
+        if ((parts.size() == 2) && !parts[0].empty() && !parts[1].empty() && string::StrToValue(parts[1], port)) {
+            serial_ = false;
+            INFO("Connecting to %s", params_.stream_.c_str());
+            return ConnectTcp(parts[0], port);
+        }
+    } else if (string::StrStartsWith(params_.stream_, "serial://")) {
+        const auto parts = string::StrSplit(params_.stream_.substr(9), ":");
+        int baudrate = 0;
+        if ((parts.size() == 2) && !parts[0].empty() && !parts[1].empty() && string::StrToValue(parts[1], baudrate)) {
+            serial_ = true;
+            INFO("Connecting to %s", params_.stream_.c_str());
+            return ConnectSerial(parts[0], baudrate);
         }
-        close(client_fd_);
     }
+
+    WARNING("Bad stream spec: %s", params_.stream_.c_str());
+    return false;
 }
 
-bool FixpositionDriver::Connect() {
-    switch (params_.fp_output.type) {
-        case INPUT_TYPE::TCP:
-            return CreateTCPSocket();
-            break;
-        case INPUT_TYPE::SERIAL:
-            return CreateSerialConnection();
-            break;
-        default:
-            std::cerr << "Unknown connection type!\n";
-            return false;
+void FixpositionDriver::Disconnect() {
+    if (IsConnected()) {
+        INFO("Disconnecting from %s", params_.stream_.c_str());
+        if (serial_) {
+            DisconnectSerial();
+        } else {
+            DisconnectTcp();
+        }
+        sensor_fd_ = -1;
+        params_.stream_.clear();
     }
 }
 
-void FixpositionDriver::WsCallback(
-    const std::unordered_map<std::string, std::vector<std::pair<bool, int>>>& sensors_meas) {
-    std::vector<FpbMeasurementsMeas> sensor_measurements;
-    for (const auto& sensor : sensors_meas) {
-        const FpbMeasurementsMeasLoc location = WsMeasStringToLoc(sensor.first);
-        if (location == MEASLOC_UNSPECIFIED) {
-            std::cerr << "Unknown measurement type will not be processed!\n";
-            continue;
-        }
-        FpbMeasurementsMeas fpb_meas;
-        if (FillWsSensorMeas(sensor.second, location, fpb_meas)) {
-            sensor_measurements.push_back(fpb_meas);
-        }
+bool FixpositionDriver::IsConnected() const { return sensor_fd_ >= 0; }
+
+bool FixpositionDriver::ConnectTcp(const std::string& ip, const int port) {
+    DisconnectTcp();
+
+    int fd = socket(AF_INET, SOCK_STREAM, 0);
+    if (fd < 0) {
+        WARNING("Failed connecting to %s: %s", params_.stream_.c_str(), string::StrError(errno).c_str());
+        return false;
     }
 
-    const size_t num_meas = sensor_measurements.size();
-    if (num_meas == 0 || num_meas > 10) {
-        std::cerr << "Number of wheel speed sensors is invalid.\n";
-        return;
+    struct sockaddr_in server_address;
+    server_address.sin_family = AF_INET;
+    server_address.sin_addr.s_addr = INADDR_ANY;
+    server_address.sin_addr.s_addr = inet_addr(ip.c_str());
+    server_address.sin_port = htons(port);
+
+    const int res = connect(fd, (struct sockaddr*)&server_address, sizeof server_address);
+
+    if (res != 0) {
+        WARNING("Failed connecting to %s: %s", params_.stream_.c_str(), string::StrError(errno).c_str());
+        return false;
     }
 
-    FpbHeader header;
-    header.sync1 = 0x66;
-    header.sync2 = 0x21;
-    header.msg_id = 2001;
-    header.payload_size = FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas);
-    header.time = 0;
-
-    FpbMeasurementsHeader meas_header;
-    meas_header.version = 1;
-    meas_header.num_meas = num_meas;
-    std::fill(&meas_header.reserved0[0], &meas_header.reserved0[6], 0);
-
-    const int msg_sz =
-        FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas) + FP_B_CRC_SIZE;
-    std::vector<uint8_t> message(msg_sz);
-
-    memcpy(&message[0], (uint8_t*)&header, sizeof(header));
-    memcpy(&message[FP_B_HEAD_SIZE], (uint8_t*)&meas_header, sizeof(meas_header));
-    for (size_t i = 0; i < num_meas; i++) {
-        memcpy(&message[FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * i)],
-               (uint8_t*)&sensor_measurements[i], sizeof(sensor_measurements[i]));
-    }
-    const uint32_t crc = Crc32fpb(
-        message.data(), FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas));
-    memcpy(&message[FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas)], &crc,
-           sizeof(crc));
-
-    switch (params_.fp_output.type) {
-        case INPUT_TYPE::TCP:
-            send(this->client_fd_, &message[0], message.size(), MSG_DONTWAIT);
-            break;
-        case INPUT_TYPE::SERIAL:
-            (void)!write(this->client_fd_, &message[0], message.size());
-            // Suppress warning - https://stackoverflow.com/a/64407070/7944565
-            break;
-        default:
-            std::cerr << "Unknown connection type!\n";
-            break;
+    INFO("Connected to %s", params_.stream_.c_str());
+    sensor_fd_ = fd;
+    poll_fds_[0].fd = fd;
+    poll_fds_[0].events = POLLIN | POLLERR | POLLHUP;
+    return true;
+}
+
+void FixpositionDriver::DisconnectTcp() {
+    if (sensor_fd_ >= 0) {
+        close(sensor_fd_);
     }
 }
 
-void FixpositionDriver::RtcmCallback(const void *rtcm_msg, const size_t msg_size) {
-    // TODO: Check that RTCM message is valid
+#ifndef B460800
+#define B460800 460800
+#endif
+#ifndef B500000
+#define B500000 500000
+#endif
+#ifndef B921600
+#define B921600 921600
+#endif
 
-    switch (params_.fp_output.type) {
-        case INPUT_TYPE::TCP:
-            send(this->client_fd_, rtcm_msg, msg_size, MSG_DONTWAIT);
-            break;
-        case INPUT_TYPE::SERIAL:
-            (void)!write(this->client_fd_, rtcm_msg, msg_size);
-            // Suppress warning - https://stackoverflow.com/a/64407070/7944565
-            break;
+bool FixpositionDriver::ConnectSerial(const std::string& dev, const int baud) {
+    speed_t speed;
+    switch (baud) {  // clang-format off
+        case   9600: speed = B9600;    break;
+        case  38400: speed = B38400;   break;
+        case  57600: speed = B57600;   break;
+        case 115200: speed = B115200;  break;
+        case 230400: speed = B230400;  break;
+        case 460800: speed = B460800;  break;
+        case 500000: speed = B500000;  break;
+        case 921600: speed = B921600;  break;  // clang-format on
         default:
-            std::cerr << "Unknown connection type!\n";
-            break;
+            WARNING("Unsupported baudrate %d", baud);
+            return false;
     }
-}
 
-bool FixpositionDriver::FillWsSensorMeas(const std::vector<std::pair<bool, int>>& meas_vec,
-                                         const FpbMeasurementsMeasLoc meas_loc, FpbMeasurementsMeas& meas_fpb) {
-    const size_t num_axis = meas_vec.size();
-    if (num_axis != 3) {
-        std::cerr << "Wheelspeed sensor has an invalid number of measurements.\n";
+    DisconnectSerial();
+
+    int fd = open(dev.c_str(), O_RDWR | O_NOCTTY);
+
+    if (fd == -1) {
+        WARNING("Failed connecting to %s: %s", params_.stream_.c_str(), string::StrError(errno).c_str());
         return false;
     }
-    meas_fpb.meas_type = MEASTYPE_VELOCITY;
-    meas_fpb.meas_loc = meas_loc;
-    std::fill(&meas_fpb.reserved1[0], &meas_fpb.reserved1[4], 0);
-    // In the current setup, the sensor will handle the timestamping as time of arrival.
-    meas_fpb.timestamp_type = TIME_TOA;
-    meas_fpb.gps_wno = 0;
-    meas_fpb.gps_tow = 0;
-    meas_fpb.meas_x_valid = meas_vec[0].first;
-    meas_fpb.meas_x = meas_vec[0].second;
-    meas_fpb.meas_y_valid = meas_vec[1].first;
-    meas_fpb.meas_y = meas_vec[1].second;
-    meas_fpb.meas_z_valid = meas_vec[2].first;
-    meas_fpb.meas_z = meas_vec[2].second;
+
+    // Get current serial port options:
+    struct termios options;
+    tcgetattr(fd, &options);
+    options_save_ = options;
+
+    char speed_buf[10];
+    snprintf(speed_buf, sizeof(speed_buf), "0%06o", (int)cfgetispeed(&options));
+
+    options.c_iflag &= ~(IXOFF | IXON | ICRNL);
+    options.c_oflag &= ~(OPOST | ONLCR);
+    options.c_lflag &= ~(ISIG | ICANON | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | IEXTEN);
+    options.c_cc[VEOL] = 0;
+    options.c_cc[VMIN] = 0;
+    options.c_cc[VTIME] = 50;
+
+    cfsetospeed(&options, speed); /* baud rate */
+    tcsetattr(fd, TCSANOW, &options);
+
+    INFO("Connected to %s", params_.stream_.c_str());
+    sensor_fd_ = fd;
+    poll_fds_[0].fd = fd;
+    poll_fds_[0].events = POLLIN | POLLERR | POLLHUP;
     return true;
 }
 
-FpbMeasurementsMeasLoc FixpositionDriver::WsMeasStringToLoc(const std::string& meas_loc) {
-    if (meas_loc == "RC") return MEASLOC_RC;
-    if (meas_loc == "FR") return MEASLOC_FR;
-    if (meas_loc == "FL") return MEASLOC_FL;
-    if (meas_loc == "RR") return MEASLOC_RR;
-    if (meas_loc == "RL") return MEASLOC_RL;
-    return MEASLOC_UNSPECIFIED;
+void FixpositionDriver::DisconnectSerial() {
+    if (sensor_fd_ >= 0) {
+        tcsetattr(sensor_fd_, TCSANOW, &options_save_);
+        close(sensor_fd_);
+    }
 }
 
-bool FixpositionDriver::InitializeConverters() {
-    for (const auto& format : params_.fp_output.formats) {
-        
-        // FP_A messages
-        if (format == "ODOMETRY") {
-            a_converters_["ODOMETRY"] = std::unique_ptr<NmeaConverter<FP_ODOMETRY>>(new NmeaConverter<FP_ODOMETRY>());
-        } else if (format == "ODOMENU") {
-            a_converters_["ODOMENU"] = std::unique_ptr<NmeaConverter<FP_ODOMENU>>(new NmeaConverter<FP_ODOMENU>());
-        } else if (format == "ODOMSH") {
-            a_converters_["ODOMSH"] = std::unique_ptr<NmeaConverter<FP_ODOMSH>>(new NmeaConverter<FP_ODOMSH>());
-        } else if (format == "ODOMSTATUS") {
-            a_converters_["ODOMSTATUS"] = std::unique_ptr<NmeaConverter<FP_ODOMSTATUS>>(new NmeaConverter<FP_ODOMSTATUS>());
-        } else if (format == "LLH") {
-            a_converters_["LLH"] = std::unique_ptr<NmeaConverter<FP_LLH>>(new NmeaConverter<FP_LLH>());
-        } else if (format == "TF") {
-            a_converters_["TF"] = std::unique_ptr<NmeaConverter<FP_TF>>(new NmeaConverter<FP_TF>());
-        } else if (format == "TP") {
-            a_converters_["TP"] = std::unique_ptr<NmeaConverter<FP_TP>>(new NmeaConverter<FP_TP>());
-        } else if (format == "RAWIMU") {
-            a_converters_["RAWIMU"] = std::unique_ptr<NmeaConverter<FP_RAWIMU>>(new NmeaConverter<FP_RAWIMU>());
-        } else if (format == "CORRIMU") {
-            a_converters_["CORRIMU"] = std::unique_ptr<NmeaConverter<FP_CORRIMU>>(new NmeaConverter<FP_CORRIMU>());
-        } else if (format == "IMUBIAS") {
-            a_converters_["IMUBIAS"] = std::unique_ptr<NmeaConverter<FP_IMUBIAS>>(new NmeaConverter<FP_IMUBIAS>());
-        } else if (format == "GNSSANT") {
-            a_converters_["GNSSANT"] = std::unique_ptr<NmeaConverter<FP_GNSSANT>>(new NmeaConverter<FP_GNSSANT>());
-        } else if (format == "GNSSCORR") {
-            a_converters_["GNSSCORR"] = std::unique_ptr<NmeaConverter<FP_GNSSCORR>>(new NmeaConverter<FP_GNSSCORR>());
-        } else if (format == "TEXT") {
-            a_converters_["TEXT"] = std::unique_ptr<NmeaConverter<FP_TEXT>>(new NmeaConverter<FP_TEXT>());
-        } else if (format == "EOE") {
-            a_converters_["EOE"] = std::unique_ptr<NmeaConverter<FP_EOE>>(new NmeaConverter<FP_EOE>());
-        
-        // NMEA messages
-        } else if (format == "GPGGA") {
-            a_converters_["GPGGA"] = std::unique_ptr<NmeaConverter<GP_GGA>>(new NmeaConverter<GP_GGA>());
-        } else if (format == "GPGLL") {
-            a_converters_["GPGLL"] = std::unique_ptr<NmeaConverter<GP_GLL>>(new NmeaConverter<GP_GLL>());
-        } else if (format == "GNGSA") {
-            a_converters_["GNGSA"] = std::unique_ptr<NmeaConverter<GN_GSA>>(new NmeaConverter<GN_GSA>());
-        } else if (format == "GPGST") {
-            a_converters_["GPGST"] = std::unique_ptr<NmeaConverter<GP_GST>>(new NmeaConverter<GP_GST>());
-        } else if (format == "GXGSV") {
-            a_converters_["GXGSV"] = std::unique_ptr<NmeaConverter<GX_GSV>>(new NmeaConverter<GX_GSV>());
-        } else if (format == "GPHDT") {
-            a_converters_["GPHDT"] = std::unique_ptr<NmeaConverter<GP_HDT>>(new NmeaConverter<GP_HDT>());
-        } else if (format == "GPRMC") {
-            a_converters_["GPRMC"] = std::unique_ptr<NmeaConverter<GP_RMC>>(new NmeaConverter<GP_RMC>());
-        } else if (format == "GPVTG") {
-            a_converters_["GPVTG"] = std::unique_ptr<NmeaConverter<GP_VTG>>(new NmeaConverter<GP_VTG>());
-        } else if (format == "GPZDA") {
-            a_converters_["GPZDA"] = std::unique_ptr<NmeaConverter<GP_ZDA>>(new NmeaConverter<GP_ZDA>());
+std::size_t FixpositionDriver::Read(uint8_t* buf, const std::size_t size, const int timeout) {
+    if (!IsConnected()) {
+        WARNING_THR(1000, "no connection, cannot read");
+        return 0;
+    }
+
+    const int res = poll(poll_fds_.data(), poll_fds_.size(), timeout);
+    // Something's wrong
+    if ((res < 0) && (errno != EINTR)) {
+        WARNING("poll() fail: %s", string::StrError(errno).c_str());
+    }
+    // We should be able to read some data
+    else if ((poll_fds_[0].revents & POLLIN) != 0) {
+        ssize_t rv = 0;
+        if (serial_) {
+            rv = recv(sensor_fd_, buf, size, MSG_DONTWAIT);
         } else {
-            std::cerr << "Unknown input format: " << format << "\n";
+            rv = read(sensor_fd_, buf, sizeof(buf));
         }
+        // We have some data
+        if (rv >= 0) {
+            return rv;
+        }
+        // Perhaps a problem
+        else {
+            if (errno == EAGAIN) {
+                return 0;
+            } else {
+                WARNING("read/recv fail: %s", string::StrError(errno).c_str());
+            }
+        }
+    }
+    // Connection is broken
+    else if ((poll_fds_[0].revents & (POLLHUP | POLLERR)) != 0) {
+        WARNING("poll() fail");
+    }
+    // Timeout, no data at the moment
+    else {
+        return 0;
     }
-    return !a_converters_.empty();
+
+    Disconnect();
+    return 0;
 }
-bool FixpositionDriver::RunOnce() {
-    if ((client_fd_ > 0) && (connection_status_ == 0) && ReadAndPublish()) {
-        return true;
+
+bool FixpositionDriver::Write(const uint8_t* buf, const std::size_t size) {
+    if (!IsConnected()) {
+        WARNING_THR(1000, "no connection, cannot write");
+        return 0;
+    }
+
+    int res = 0;
+    if (serial_) {
+        res = write(this->sensor_fd_, buf, size);
     } else {
-        close(client_fd_);
-        client_fd_ = -1;
-        return false;
+        res = send(this->sensor_fd_, buf, size, MSG_DONTWAIT);
     }
+    if (res < 0) {
+        WARNING("send/write fail: %s", string::StrError(errno).c_str());
+        return 0;
+    }
+    return (res > 0) && (res == (int)size);
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+bool FixpositionDriver::StartDriver() { return Connect() && worker_.Start(); }
+
+void FixpositionDriver::StopDriver() {
+    worker_.Stop();
+    Disconnect();
 }
 
-bool FixpositionDriver::ReadAndPublish() {
-    int msg_size = 0;
-    
-    // Extract messages until the buffer is empty
-    while (buf_size > 0) {
-        // Check whether the message is NOV_B
-        msg_size = IsNovMessage(readBuf, buf_size);
-        
-        // a) Message is NOV_B --> Process message and remove it from the buffer
-        if (msg_size > 0) {
-            NovConvertAndPublish(readBuf);
-            buf_size -= msg_size;
-            if (buf_size > 0) {
-                memmove(readBuf, &readBuf[msg_size], buf_size);
+void FixpositionDriver::Worker(void* /*arg*/) {
+    INFO("Driver running...");
+
+    while (!worker_.ShouldAbort()) {
+        // While we're connected to the sensor...
+        if (IsConnected()) {
+            // Read more data from sensor and feed the parser
+            uint8_t buf[parser::MAX_ADD_SIZE];
+            const std::size_t size = Read(buf, sizeof(buf), 337);
+            if (size == 0) {
+                continue;  // try again
             }
-            // Look for new messages in the buffer
-        }
-        
-        // b) Message is not NOV_B. Check whether it is NMEA/FP_A
-        else if (msg_size == 0) {
-            msg_size = IsNmeaMessage((char*)readBuf, buf_size);
-            
-            // Message is NMEA/FP_A --> Process message and remove it from the buffer
-            if (msg_size > 0) {
-                NmeaConvertAndPublish({(const char*)readBuf, (const char*)readBuf + msg_size});
-                buf_size -= msg_size;
-                if (buf_size > 0) {
-                    memmove(readBuf, &readBuf[msg_size], buf_size);
-                }
-                // Look for new messages in the buffer
+            if (!parser_.Add(buf, size)) {
+                WARNING("Parser overflow");  // should not happen, as we respect the parser's limits (MAX_ADD_SIZE)
+                parser_.Reset();
+                parser_.Add(buf, size);
             }
-            
-            // Message is neither NMEA/FP_A or NOV_B --> Remove one byte from the buffer and try again until it is empty
-            else if (msg_size == 0) {
-                if (buf_size > 0) {
-                    buf_size -= 1;
-                    memmove(readBuf, &readBuf[1], buf_size);
-                }
-                
-                // Buffer is empty --> Wait for more data
-                else /* buf_size == 0 */ {
-                    break;
+
+            // Process received message(s)
+            parser::ParserMsg msg;
+            while (parser_.Process(msg)) {
+                IF_TRACE(msg.MakeInfo());
+                TRACE("received %s %" PRIuMAX " -- %s", msg.name_.c_str(), msg.data_.size(), msg.info_.c_str());
+                switch (msg.proto_) {
+                    case parser::Protocol::FP_A:
+                        NotifyFpaObservers(msg);
+                        break;
+                    case parser::Protocol::NMEA:
+                        NotifyNmeaObservers(msg);
+                        break;
+                    case parser::Protocol::NOV_B:
+                        NotifyNovbObservers(msg);
+                        break;
+                    case parser::Protocol::FP_B:
+                    case parser::Protocol::UBX:
+                    case parser::Protocol::RTCM3:
+                    case parser::Protocol::UNI_B:
+                    case parser::Protocol::SPARTN:
+                    case parser::Protocol::OTHER:
+                        break;
                 }
+                NotifyRawObservers(msg);
             }
-            
-            // NMEA message might be incomplete --> Wait for more data
-            else /* msg_size < 0 */ {
+        }
+        // Reconnect after some time...
+        else {
+            INFO("Reconnecting in %.1f seconds...", params_.reconnect_delay_);
+            if (worker_.Sleep(params_.reconnect_delay_ * 1000)) {
                 break;
             }
-        }
-
-        // c) NOV_B message might be incomplete --> Wait for more data
-        else /* msg_size < 0 */ {
-            break;
+            Connect();
         }
     }
+}
 
-    // Read more data from the TCP/Serial port
-    int rem_size = sizeof(readBuf) - buf_size;
-    if (rem_size > 0) {
-        ssize_t rv;
-        if (params_.fp_output.type == INPUT_TYPE::TCP) {
-            rv = recv(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size, MSG_DONTWAIT);
-        } else if (params_.fp_output.type == INPUT_TYPE::SERIAL) {
-            rv = read(client_fd_, (void*)&readBuf[buf_size], sizeof(readBuf) - buf_size);
-        } else {
-            rv = 0;
-        }
-
-        if (rv == 0) {
-            std::cerr << "Connection closed.\n";
-            return false;
-        }
-        
-        if (rv < 0 && errno == EAGAIN) {
-            /* no data for now, call back when the socket is readable */
-            return true;
-        } 
-        
-        if (rv < 0) {
-            std::cerr << "Connection error.\n";
-            return false;
-        }
+// ---------------------------------------------------------------------------------------------------------------------
 
-        buf_size += rv;
+void FixpositionDriver::AddFpaObserver(const std::string& message_name, FpaObserver observer) {
+    DEBUG("Adding FP_A observer for %s", message_name.c_str());
+    auto entry = fpa_observers_.find(message_name);
+    if (entry == fpa_observers_.end()) {
+        entry = fpa_observers_.insert({message_name, {}}).first;
     }
-
-    return true;
+    entry->second.push_back(observer);
 }
 
-void FixpositionDriver::NmeaConvertAndPublish(const std::string& msg) {
-    // split the msg into tokens, removing the *XX checksum
-    std::vector<std::string> tokens;
-    std::size_t star_pos = msg.find_last_of("*");
-    SplitMessage(tokens, msg.substr(1, star_pos - 1), ",");
-
-    // if it doesn't start with FP then do nothing
-    if ((tokens.at(0) != "FP") && (tokens.at(0) != "GPGGA") && 
-        (tokens.at(0) != "GPGLL") && (tokens.at(0) != "GNGSA") &&
-        (tokens.at(0) != "GPGST") && (tokens.at(0) != "GPHDT") &&
-        (tokens.at(0) != "GPRMC") && (tokens.at(0) != "GPVTG") &&
-        (tokens.at(0) != "GPZDA") && (tokens.at(0) != "GPGSV") &&
-        (tokens.at(0) != "GAGSV") && (tokens.at(0) != "GBGSV") &&
-        (tokens.at(0) != "GLGSV")) {
-        return;
-    }
-
-    // Get the header of the sentence
-    std::string _header;
-    if (tokens.at(0) == "GPGGA") {
-        _header = "GPGGA";
-    } else if (tokens.at(0) == "GPGLL") {
-        _header = "GPGLL";
-    } else if (tokens.at(0) == "GNGSA") {
-        _header = "GNGSA";
-    } else if (tokens.at(0) == "GPGST") {
-        _header = "GPGST";
-    } else if (tokens.at(0) == "GPHDT") {
-        _header = "GPHDT";
-    } else if (tokens.at(0) == "GPRMC") {
-        _header = "GPRMC";
-    } else if (tokens.at(0) == "GPVTG") {
-        _header = "GPVTG";
-    } else if (tokens.at(0) == "GPZDA") {
-        _header = "GPZDA";
-    } else if (tokens.at(0) == "GPGSV" || tokens.at(0) == "GAGSV" || tokens.at(0) == "GBGSV" || tokens.at(0) == "GLGSV") {
-        _header = "GXGSV";
-    } else {
-        _header = tokens.at(1);
+void FixpositionDriver::RemoveFpaObservers() { fpa_observers_.clear(); }
+
+void FixpositionDriver::NotifyFpaObservers(const ParserMsg& msg) {
+    auto entry = fpa_observers_.find(msg.name_);
+    if ((entry != fpa_observers_.end()) && !entry->second.empty()) {
+        TRACE("notify fpa %s", msg.name_.c_str());
+
+#define _DISPATCH(_type_)                                                                                          \
+    else if (msg.name_ == _type_::MSG_NAME) {                                                                      \
+        _type_ payload;                                                                                            \
+        if (payload.SetFromMsg(msg.data_.data(), msg.data_.size())) {                                              \
+            for (auto& obs : entry->second) {                                                                      \
+                obs(payload);                                                                                      \
+            }                                                                                                      \
+        } else {                                                                                                   \
+            msg.MakeInfo();                                                                                        \
+            WARNING_THR(1000, "Failed decoding %s: %s", msg.name_.c_str(), msg.info_.c_str());                     \
+            TRACE_HEXDUMP(msg.data_.data(), msg.data_.size(), "    ", "Failed decoding %s: %s", msg.name_.c_str(), \
+                          msg.info_.c_str());                                                                      \
+        }                                                                                                          \
     }
-    const std::string header = _header;
 
-    // If we have a converter available, convert to ros.
-    if (a_converters_[header] != nullptr) {
-        a_converters_[header]->ConvertTokens(tokens);
+        if (false) {
+        }
+        _DISPATCH(fpa::FpaEoePayload)
+        _DISPATCH(fpa::FpaGnssantPayload)
+        _DISPATCH(fpa::FpaGnsscorrPayload)
+        _DISPATCH(fpa::FpaRawimuPayload)
+        _DISPATCH(fpa::FpaCorrimuPayload)
+        _DISPATCH(fpa::FpaImubiasPayload)
+        _DISPATCH(fpa::FpaLlhPayload)
+        _DISPATCH(fpa::FpaOdometryPayload)
+        _DISPATCH(fpa::FpaOdomshPayload)
+        _DISPATCH(fpa::FpaOdomenuPayload)
+        _DISPATCH(fpa::FpaOdomstatusPayload)
+        _DISPATCH(fpa::FpaTextPayload)
+        _DISPATCH(fpa::FpaTfPayload)
+        _DISPATCH(fpa::FpaTpPayload)
+
+#undef _DISPATCH
     }
 }
 
-void FixpositionDriver::NovConvertAndPublish(const uint8_t* msg) {
-    auto* header = reinterpret_cast<const Oem7MessageHeaderMem*>(msg);
-    const auto msg_id = header->message_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-    if (msg_id == static_cast<uint16_t>(MessageId::BESTGNSSPOS)) {
-        for (auto& ob : bestgnsspos_obs_) {
-            auto* payload = reinterpret_cast<const BESTGNSSPOSMem*>(msg + sizeof(Oem7MessageHeaderMem));
-            ob(header, payload);
-        }
+void FixpositionDriver::AddNmeaObserver(const std::string& formatter, NmeaObserver observer) {
+    DEBUG("Adding NMEA observer for %s", formatter.c_str());
+    auto entry = nmea_observers_.find(formatter);
+    if (entry == nmea_observers_.end()) {
+        entry = nmea_observers_.insert({formatter, {}}).first;
     }
-    // TODO add more msg types
+    entry->second.push_back(observer);
 }
 
-bool FixpositionDriver::CreateTCPSocket() {
-    if (client_fd_ != -1) {
-        std::cerr << "TCP connection already exists.\n";
-        return true;
-    }
-
-    client_fd_ = socket(AF_INET, SOCK_STREAM, 0);
+void FixpositionDriver::RemoveNmeaObservers() { nmea_observers_.clear(); }
 
-    if (client_fd_ < 0) {
-        std::cerr << "Error in client creation.\n";
-        return false;
-    } else {
-        std::cout << "Client created.\n";
+void FixpositionDriver::NotifyNmeaObservers(const fpsdk::common::parser::ParserMsg& msg) {
+    // NMEA observers are registered using the formatter (e.g. "RMC"), ignoring the talker ("GP", "GN", etc.)
+    nmea::NmeaMessageMeta meta;
+    if (!nmea::NmeaGetMessageMeta(meta, msg.data_.data(), msg.data_.size())) {
+        WARNING_THR(1000, "Failed decoding %s", msg.name_.c_str());
+        return;
     }
 
-    struct sockaddr_in server_address;
-    server_address.sin_family = AF_INET;
-    server_address.sin_addr.s_addr = INADDR_ANY;
-    server_address.sin_port = htons(std::stoi(params_.fp_output.port));
-    server_address.sin_addr.s_addr = inet_addr(params_.fp_output.ip.c_str());
-
-    connection_status_ = connect(client_fd_, (struct sockaddr*)&server_address, sizeof server_address);
+    auto entry = nmea_observers_.find(meta.formatter_);
+    if ((entry != nmea_observers_.end()) && !entry->second.empty()) {
+        TRACE("notify nmea %s (%s)", meta.formatter_, msg.name_.c_str());
+
+#define _DISPATCH(_type_)                                                                                          \
+    else if (std::strcmp(meta.formatter_, _type_::FORMATTER) == 0) {                                               \
+        _type_ payload;                                                                                            \
+        if (payload.SetFromMsg(msg.data_.data(), msg.data_.size())) {                                              \
+            for (auto& obs : entry->second) {                                                                      \
+                obs(payload);                                                                                      \
+            }                                                                                                      \
+        } else {                                                                                                   \
+            msg.MakeInfo();                                                                                        \
+            WARNING_THR(1000, "Bad %s: %s", msg.name_.c_str(), msg.info_.c_str());                                 \
+            TRACE_HEXDUMP(msg.data_.data(), msg.data_.size(), "    ", "Failed decoding %s: %s", msg.name_.c_str(), \
+                          msg.info_.c_str());                                                                      \
+        }                                                                                                          \
+    }
 
-    if (connection_status_ != 0) {
-        std::cerr << "Error on connection of TCP socket: " << strerror(errno) << "\n";
-        return false;
+        if (false) {
+        }
+        _DISPATCH(nmea::NmeaGgaPayload)
+        _DISPATCH(nmea::NmeaGllPayload)
+        _DISPATCH(nmea::NmeaRmcPayload)
+        _DISPATCH(nmea::NmeaVtgPayload)
+        _DISPATCH(nmea::NmeaGstPayload)
+        _DISPATCH(nmea::NmeaHdtPayload)
+        _DISPATCH(nmea::NmeaZdaPayload)
+        _DISPATCH(nmea::NmeaGsaPayload)
+        _DISPATCH(nmea::NmeaGsvPayload)
+
+#undef _DISPATCH
     }
-    return true;
 }
 
-bool FixpositionDriver::CreateSerialConnection() {
-    if (client_fd_ != -1) {
-        std::cerr << "Serial connection already exists.\n";
-        return true;
-    }
+// ---------------------------------------------------------------------------------------------------------------------
 
-    client_fd_ = open(params_.fp_output.port.c_str(), O_RDWR | O_NOCTTY);
+void FixpositionDriver::AddNovbObserver(const std::string& message_name, NovbObserver observer) {
+    DEBUG("Adding NOV_B observer for %s", message_name.c_str());
+    auto entry = novb_observers_.find(message_name);
+    if (entry == novb_observers_.end()) {
+        entry = novb_observers_.insert({message_name, {}}).first;
+    }
+    entry->second.push_back(observer);
+}
 
-    struct termios options;
-    speed_t speed;
+void FixpositionDriver::RemoveNovbObservers() { novb_observers_.clear(); }
 
-    switch (params_.fp_output.baudrate) {
-        case 9600:
-            speed = B9600;
-            break;
+void FixpositionDriver::NotifyNovbObservers(const fpsdk::common::parser::ParserMsg& msg) {
+    auto entry = novb_observers_.find(msg.name_);
+    if ((entry != novb_observers_.end()) && !entry->second.empty()) {
+        TRACE("notify novb %s", msg.name_.c_str());
 
-        case 38400:
-            speed = B38400;
-            break;
+        const uint8_t* frame = msg.data_.data();
+        const novb::NovbHeader* header = (novb::NovbHeader*)&frame[0];
+        const uint8_t* payload =
+            (header->IsLongHeader() ? &frame[novb::NOV_B_HEAD_SIZE_LONG] : &frame[novb::NOV_B_HEAD_SIZE_SHORT]);
+        for (auto& obs : entry->second) {
+            obs(header, payload);
+        }
+    }
+}
 
-        case 57600:
-            speed = B57600;
-            break;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        case 115200:
-            speed = B115200;
-            break;
+void FixpositionDriver::AddRawObserver(RawObserver observer) {
+    DEBUG("Adding observer for raw messages");
+    raw_observers_.push_back(observer);
+}
 
-        case 230400:
-            speed = B230400;
-            break;
+void FixpositionDriver::NotifyRawObservers(const fpsdk::common::parser::ParserMsg& msg) {
+    if (!raw_observers_.empty()) {
+        TRACE("notify raw %s", msg.name_.c_str());
+        for (auto& obs : raw_observers_) {
+            obs(msg);
+        }
+    }
+}
 
-        case 460800:
-            speed = B460800;
-            break;
+void FixpositionDriver::RemoveRawObservers() { raw_observers_.clear(); }
 
-        case 500000:
-            speed = B500000;
-            break;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        case 921600:
-            speed = B921600;
-            break;
+void FixpositionDriver::SendCorrectionData(const uint8_t* msg, const std::size_t size) {
+    // TODO: Check that RTCM message is valid. Maybe run the data through a Parser?
+    TRACE("Send correction data (%" PRIuMAX " bytes)", size);
+    if (!Write(msg, size)) {
+        WARNING_THR(1000, "Failed sending correction data");
+    }
+}
 
-        case 1000000:
-            speed = B1000000;
+// ---------------------------------------------------------------------------------------------------------------------
+
+void FixpositionDriver::SendWheelspeedData(const std::vector<WheelSpeedData>& data) {
+    TRACE("Send wheelspeed data (#data=%" PRIuMAX ")", data.size());
+
+    uint8_t payload[fpb::FP_B_MEASUREMENTS_HEAD_SIZE +
+                    (fpb::FP_B_MEASUREMENTS_MAX_NUM_MEAS * fpb::FP_B_MEASUREMENTS_MEAS_SIZE)];
+
+    fpb::FpbMeasurementsHead head = {0};
+    head.version = fpb::FP_B_MEASUREMENTS_V1;
+    std::memcpy(&payload[0], &head, sizeof(head));
+    std::size_t payload_size = sizeof(head);
+
+    std::size_t n_meas = 0;
+    bool meas_ok = true;
+    for (auto& wheel : data) {
+        fpb::FpbMeasurementsMeas meas = {0};
+
+        meas.meas_type = types::EnumToVal(fpb::FpbMeasurementsMeasType::VELOCITY);
+        // clang-format off
+        if      (wheel.location_ == "RC") { meas.meas_loc = types::EnumToVal(parser::fpb::FpbMeasurementsMeasLoc::RC); }
+        else if (wheel.location_ == "FR") { meas.meas_loc = types::EnumToVal(parser::fpb::FpbMeasurementsMeasLoc::FR); }
+        else if (wheel.location_ == "FL") { meas.meas_loc = types::EnumToVal(parser::fpb::FpbMeasurementsMeasLoc::FL); }
+        else if (wheel.location_ == "RR") { meas.meas_loc = types::EnumToVal(parser::fpb::FpbMeasurementsMeasLoc::RR); }
+        else if (wheel.location_ == "RL") { meas.meas_loc = types::EnumToVal(parser::fpb::FpbMeasurementsMeasLoc::RL); }
+        else                              { meas.meas_loc = types::EnumToVal(parser::fpb::FpbMeasurementsMeasLoc::UNSPECIFIED); meas_ok = false; }
+        // clang-format on
+        meas.timestamp_type = types::EnumToVal(fpb::FpbMeasurementsTimestampType::TIMEOFARRIVAL);
+        meas.meas_x_valid = wheel.vx_valid_;
+        meas.meas_x = wheel.vx_;
+        meas.meas_y_valid = wheel.vy_valid_;
+        meas.meas_y = wheel.vy_;
+        meas.meas_z_valid = wheel.vz_valid_;
+        meas.meas_z = wheel.vz_;
+
+        std::memcpy(&payload[fpb::FP_B_MEASUREMENTS_HEAD_SIZE + (fpb::FP_B_MEASUREMENTS_MEAS_SIZE * n_meas)], &meas,
+                    sizeof(meas));
+        payload_size += sizeof(meas);
+
+        n_meas++;
+        if (n_meas >= fpb::FP_B_MEASUREMENTS_MAX_NUM_MEAS) {
+            meas_ok = false;
             break;
-
-        default:
-            speed = B115200;
-            std::cerr << "Unsupported baudrate: " << params_.fp_output.baudrate
-                      << "\n\tsupported examples:\n\t9600, "
-                         "19200, "
-                         "38400, "
-                         "57600\t\n115200\n230400\n460800\n500000\n921600\n1000000\n";
+        }
+    }
+    if (n_meas == 0) {
+        meas_ok = false;
+    }
+    if (!meas_ok) {
+        WARNING_THR(1000, "Invalid WheelSpeedData (#data=%" PRIuMAX ", #meas=%" PRIuMAX ")", data.size(), n_meas);
     }
 
-    if (client_fd_ == -1) {
-        // Could not open the port.
-        std::cerr << "Failed to open serial port " << strerror(errno) << "\n";
-        return false;
+    std::vector<uint8_t> message;
+    if (parser::fpb::FpbMakeMessage(message, parser::fpb::FP_B_MEASUREMENTS_MSGID, 0, payload, payload_size)) {
+        if (!Write(message.data(), message.size())) {
+            WARNING_THR(1000, "Failed sending FP_B-MEASUREMENTS message");
+        }
     } else {
-        // Get current serial port options:
-        tcgetattr(client_fd_, &options);
-        options_save_ = options;
-        char speed_buf[10];
-        snprintf(speed_buf, sizeof(speed_buf), "0%06o", (int)cfgetispeed(&options));
-
-        options.c_iflag &= ~(IXOFF | IXON | ICRNL);
-        options.c_oflag &= ~(OPOST | ONLCR);
-        options.c_lflag &= ~(ISIG | ICANON | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | IEXTEN);
-        options.c_cc[VEOL] = 0;
-        options.c_cc[VMIN] = 0;
-        options.c_cc[VTIME] = 50;
-
-        cfsetospeed(&options, speed); /* baud rate */
-        tcsetattr(client_fd_, TCSANOW, &options);
-        connection_status_ = 0;  // not used for serial, set to 0 (success)
-        return true;
+        WARNING_THR(1000, "Failed making FP_B-MEASUREMENTS message");
     }
 }
+
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_driver_lib/src/gnss_tf.cpp b/fixposition_driver_lib/src/gnss_tf.cpp
deleted file mode 100644
index 309d32a..0000000
--- a/fixposition_driver_lib/src/gnss_tf.cpp
+++ /dev/null
@@ -1,197 +0,0 @@
-/**
- *  @file
- *  @brief Helper functions
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/gnss_tf.hpp>
-
-namespace fixposition {
-
-namespace Constants {
-// static constexpr values representing the earth WGS84
-// https://en.wikipedia.org/wiki/World_Geodetic_System#Defining_Parameters
-static constexpr double wgs84_a = 6378137.0;           //!< earth radius major axis [m]
-static constexpr double wgs84_b = 6356752.314245;      //!< earth radius minor axis [m]
-static constexpr double wgs84_inv_f = 298.257223563;   //!< 1/f inverse of flattening parameter
-static constexpr double wgs84_e_2 = 6.69437999014e-3;  //!< first eccentricity Squared
-
-static constexpr double wgs84_a_2 = wgs84_a * wgs84_a;                //!< a_^2
-static constexpr double wgs84_b_2 = wgs84_b * wgs84_b;                //!< a_^2
-static constexpr double wgs84_e_prime_2 = wgs84_a_2 / wgs84_b_2 - 1;  //!< e'^2 second eccentricity squared
-}  // namespace Constants
-
-/**
- * @details
- *
- * \f[
- *
- * \text{Rotate a vector from ECEF to ENU:} \\
- * V_{ENU} = M \cdot V_{ECEF} \\
- * \text{Rotate a covariance matrix from ECEF to ENU:} \\
- * Cov_{ENU} = M \cdot Cov_{ECEF} \cdot M^{T} \\
- *
- * \f]
- *
- * Reference https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
- *
- */
-Eigen::Matrix3d RotEnuEcef(double lat, double lon) {
-    const double s_lon = sin(lon);
-    const double c_lon = cos(lon);
-    const double s_lat = sin(lat);
-    const double c_lat = cos(lat);
-    const double m00 = -s_lon;
-    const double m01 = c_lon;
-    const double m02 = 0;
-    const double m10 = -c_lon * s_lat;
-    const double m11 = -s_lon * s_lat;
-    const double m12 = c_lat;
-    const double m20 = c_lon * c_lat;
-    const double m21 = s_lon * c_lat;
-    const double m22 = s_lat;
-
-    Eigen::Matrix3d res;
-    // This initializes the matrix row by row
-    res << m00, m01, m02, m10, m11, m12, m20, m21, m22;
-
-    return res;
-}
-
-/**
- * @details
- *
- * \f[
- *
- * \text{Rotate a vector from ECEF to ENU:} \\
- * V_{ENU} = M \cdot V_{ECEF} \\
- * \text{Rotate a covariance matrix from ECEF to ENU:} \\
- * Cov_{ENU} = M \cdot Cov_{ECEF} \cdot M^{T} \\
- *
- * \f]
- *
- */
-Eigen::Matrix3d RotEnuEcef(const Eigen::Vector3d &ecef) {
-    const Eigen::Vector3d wgs84llh = TfWgs84LlhEcef(ecef);
-    return RotEnuEcef(wgs84llh.x(), wgs84llh.y());
-}
-
-Eigen::Matrix3d RotNedEnu() {
-    /**
-     * | 0, 1, 0 |
-     * | 1, 0, 0 |
-     * | 0, 0,-1 |
-     *
-     */
-
-    Eigen::Matrix3d rot_ned_enu;
-    rot_ned_enu << 0, 1, 0, 1, 0, 0, 0, 0, -1;
-    return rot_ned_enu;
-}
-
-Eigen::Matrix3d RotNedEcef(double lat, double lon) { return RotNedEnu() * RotEnuEcef(lat, lon); }
-
-Eigen::Matrix3d RotNedEcef(const Eigen::Vector3d &ecef) {
-    const Eigen::Vector3d wgs84llh = TfWgs84LlhEcef(ecef);
-    return RotNedEcef(wgs84llh.x(), wgs84llh.y());
-}
-
-Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref) {
-    const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref);
-    return RotEnuEcef(wgs84llh_ref.x(), wgs84llh_ref.y()) * (ecef - ecef_ref);
-}
-
-Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d &enu, const Eigen::Vector3d &wgs84llh_ref) {
-    const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref);
-    return ecef_ref + RotEnuEcef(wgs84llh_ref.x(), wgs84llh_ref.y()).transpose() * enu;
-}
-
-Eigen::Vector3d TfNedEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref) {
-    const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref);
-    return RotNedEcef(wgs84llh_ref.x(), wgs84llh_ref.y()) * (ecef - ecef_ref);
-}
-
-Eigen::Vector3d TfEcefNed(const Eigen::Vector3d &ned, const Eigen::Vector3d &wgs84llh_ref) {
-    const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref);
-    return ecef_ref + RotNedEcef(wgs84llh_ref.x(), wgs84llh_ref.y()).transpose() * ned;
-}
-/**
- * @details Implementation based on paper
- * https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution
- *
- */
-Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d &ecef) {
-    const double x = ecef.x();
-    const double y = ecef.y();
-    const double z = ecef.z();
-
-    const double x_2 = x * x;  //!< x^2
-    const double y_2 = y * y;  //!< y^2
-    const double z_2 = z * z;  //!< z^2
-
-    const double r_2 = (x_2 + y_2);  //!< r^2
-    const double r = sqrt(r_2);      //!< r
-
-    const double F = 54.0 * Constants::wgs84_b_2 * z_2;
-    const double G =
-        r_2 + (1 - Constants::wgs84_e_2) * z_2 - Constants::wgs84_e_2 * (Constants::wgs84_a_2 - Constants::wgs84_b_2);
-    const double c = Constants::wgs84_e_2 * Constants::wgs84_e_2 * F * r_2 / (G * G * G);
-    const double s = cbrt(1 + c + sqrt(c * c + 2 * c));
-    const double P = F / (3.0 * (s + 1.0 + 1.0 / s) * (s + 1.0 + 1.0 / s) * G * G);
-    const double Q = sqrt(1 + 2 * Constants::wgs84_e_2 * Constants::wgs84_e_2 * P);
-    const double r0 = -P * Constants::wgs84_e_2 * r / (1 + Q) +
-                      sqrt(0.5 * Constants::wgs84_a_2 * (1.0 + 1.0 / Q) -
-                           (P * (1 - Constants::wgs84_e_2) * z_2 / (Q + Q * Q)) - 0.5 * P * r_2);
-    const double t1 = (r - Constants::wgs84_e_2 * r0);
-    const double t1_2 = t1 * t1;
-    const double U = sqrt(t1_2 + z_2);
-    const double V = sqrt(t1_2 + (1 - Constants::wgs84_e_2) * z_2);
-    const double a_V = Constants::wgs84_a * V;
-    const double z0 = Constants::wgs84_b_2 * z / a_V;
-
-    const double h = U * (1 - Constants::wgs84_b_2 / a_V);
-    const double lat = atan((z + Constants::wgs84_e_prime_2 * z0) / r);
-    const double lon = atan2(y, x);
-
-    return Eigen::Vector3d(lat, lon, h);
-}
-
-Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d &wgs84llh) {
-    const double lat = wgs84llh.x();
-    const double lon = wgs84llh.y();
-    const double height = wgs84llh.z();
-    const double s_lat = sin(lat);
-    const double c_lat = cos(lat);
-    const double s_lon = sin(lon);
-    const double c_lon = cos(lon);
-    // N is in meters
-    const double n = Constants::wgs84_a / sqrt(1.0 - Constants::wgs84_e_2 * s_lat * s_lat);
-    const double n_plus_height = n + height;
-
-    Eigen::Vector3d ecef;
-    ecef.x() = n_plus_height * c_lat * c_lon;
-    ecef.y() = n_plus_height * c_lat * s_lon;
-    ecef.z() = (n * (1 - Constants::wgs84_e_2) + height) * s_lat;
-
-    return ecef;
-}
-
-Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r) {
-    //! Rotation Matrix to convert to ENU frame
-    const Eigen::Matrix3d rot_enu_ecef = RotEnuEcef(ecef_p);
-    //! Convert the Pose into ENU frame
-    const Eigen::Matrix3d rot_enu_body = rot_enu_ecef * ecef_r;
-    //! Convert Rotation Matrix into Yaw-Pitch-Roll
-    return RotToEul(rot_enu_body);
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/helper.cpp b/fixposition_driver_lib/src/helper.cpp
index 222c802..589d76e 100644
--- a/fixposition_driver_lib/src/helper.cpp
+++ b/fixposition_driver_lib/src/helper.cpp
@@ -1,84 +1,333 @@
 /**
- *  @file
- *  @brief Helper functions
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Helper functions and types
  */
 
+/* LIBC/STL */
+#include <utility>
+
+/* EXTERNAL */
+#include <fpsdk_common/logging.hpp>
+#include <fpsdk_common/parser/crc.hpp>
+#include <fpsdk_common/string.hpp>
+#include <fpsdk_common/types.hpp>
+#include <fpsdk_common/utils.hpp>
+
 /* PACKAGE */
-#include <fixposition_driver_lib/helper.hpp>
-#include <fixposition_driver_lib/messages/nov_type.hpp>
+#include "fixposition_driver_lib/helper.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
+
+using namespace fpsdk::common;
+using namespace fpsdk::common::parser;
 
-static constexpr char kNmeaPreamble = '$';
-static constexpr int kLibParserMaxNmeaSize = 400;
+// ---------------------------------------------------------------------------------------------------------------------
 
-void SplitMessage(std::vector<std::string>& tokens, const std::string& msg, const std::string& delim) {
-    boost::split(tokens, msg, boost::is_any_of(delim));
+time::Time FpaGpsTimeToTime(const fpa::FpaGpsTime gps_time) {
+    time::Time time;
+    // Check time is available and check that it is in range
+    if (gps_time.week.valid && gps_time.tow.valid &&
+        time.SetWnoTow({gps_time.week.value, gps_time.tow.value, time::WnoTow::Sys::GPS})) {
+    }
+    return time;
 }
 
-void BestGnssPosToNavSatFix(const Oem7MessageHeaderMem* const header, const BESTGNSSPOSMem* const bestgnsspos,
-                            NavSatFixData& navsatfix) {
-    // Header timestamp
-    navsatfix.stamp.wno = header->gps_week;
-    navsatfix.stamp.tow = header->gps_milliseconds * 1e-3;
+// ---------------------------------------------------------------------------------------------------------------------
+
+bool PoseWithCovData::SetFromFpaOdomPayload(const fpa::FpaOdomPayload& payload) {
+    bool ok = true;
+    if (payload.pos.valid) {
+        position = {payload.pos.values[0], payload.pos.values[1], payload.pos.values[2]};
+    } else {
+        ok = false;
+    }
+    if (payload.orientation.valid) {
+        orientation = {payload.orientation.values[0], payload.orientation.values[1], payload.orientation.values[2],
+                       payload.orientation.values[3]};
+    } else {
+        ok = false;
+    }
+    if (payload.pos_cov.valid && payload.orientation_cov.valid) {
+        cov = BuildCovMat6D(payload.pos_cov.values[0], payload.pos_cov.values[1], payload.pos_cov.values[2],
+                            payload.pos_cov.values[3], payload.pos_cov.values[4], payload.pos_cov.values[5],
+                            payload.orientation_cov.values[0], payload.orientation_cov.values[1],
+                            payload.orientation_cov.values[2], payload.orientation_cov.values[3],
+                            payload.orientation_cov.values[4], payload.orientation_cov.values[5]);
+        // DEBUG_S("PoseWithCovData.cov: " << cov);
+    } else {
+        ok = false;
+    }
 
-    // Data
-    navsatfix.latitude = bestgnsspos->lat;
-    navsatfix.longitude = bestgnsspos->lon;
-    navsatfix.altitude = bestgnsspos->hgt;
+    valid = ok;
+    return ok;
+}
 
-    Eigen::Array3d cov_diag(bestgnsspos->lat_stdev, bestgnsspos->lon_stdev, bestgnsspos->hgt_stdev);
+// ---------------------------------------------------------------------------------------------------------------------
 
-    navsatfix.cov = (cov_diag * cov_diag).matrix().asDiagonal();
-    navsatfix.position_covariance_type = 2;
+bool TwistWithCovData::SetFromFpaOdomPayload(const fpa::FpaOdomPayload& payload) {
+    bool ok = true;
+    if (payload.vel.valid) {
+        linear = {payload.vel.values[0], payload.vel.values[1], payload.vel.values[2]};
+    } else {
+        ok = false;
+    }
+    if (payload.rot.valid) {
+        angular = {payload.rot.values[0], payload.rot.values[1], payload.rot.values[2]};
+    } else {
+        ok = false;
+    }
+    if (payload.vel_cov.valid) {
+        cov = BuildCovMat6D(payload.vel_cov.values[0], payload.vel_cov.values[1], payload.vel_cov.values[2],
+                            payload.vel_cov.values[3], payload.vel_cov.values[4], payload.vel_cov.values[5], 0.0, 0.0,
+                            0.0, 0.0, 0.0, 0.0);
+        // DEBUG_S("TwistWithCovData.cov: " << cov);
 
-    switch (static_cast<PositionOrVelocityType>(bestgnsspos->pos_type)) {
-        case PositionOrVelocityType::NARROW_INT:
-            navsatfix.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_GBAS_FIX);
-            break;
-        case PositionOrVelocityType::NARROW_FLOAT:
-        case PositionOrVelocityType::SINGLE:
-            navsatfix.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_FIX);
-            break;
-        default:
-            navsatfix.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_NO_FIX);
+    } else {
+        ok = false;
     }
 
-    // TODO hardcoded for now for all 4 systems
-    navsatfix.status.service = 0b000000000000000;
-    navsatfix.status.service |= 1;
-    navsatfix.status.service |= 2;
-    navsatfix.status.service |= 4;
-    navsatfix.status.service |= 8;
+    valid = ok;
+    return ok;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
 
-    switch (static_cast<MessageTypeSource>(header->message_type & static_cast<uint8_t>(MessageTypeSource::_MASK))) {
-        case MessageTypeSource::PRIMARY:
-            navsatfix.frame_id = "GNSS1";
+bool OdometryData::SetFromFpaOdomPayload(const fpa::FpaOdomPayload& payload) {
+    bool ok = true;
+    switch (payload.which) {
+        case fpa::FpaOdomPayload::Which::ODOMETRY:
+            frame_id = ODOMETRY_FRAME_ID;
+            child_frame_id = ODOMETRY_CHILD_FRAME_ID;
+            type = Type::ODOMETRY;
             break;
-        case MessageTypeSource::SECONDARY:
-            navsatfix.frame_id = "GNSS2";
+        case fpa::FpaOdomPayload::Which::ODOMSH:
+            frame_id = ODOMSH_FRAME_ID;
+            child_frame_id = ODOMSH_CHILD_FRAME_ID;
+            type = Type::ODOMSH;
             break;
-        case MessageTypeSource::_MASK:
-            navsatfix.frame_id = "GNSS";
+        case fpa::FpaOdomPayload::Which::ODOMENU:
+            frame_id = ODOMENU_FRAME_ID;
+            child_frame_id = ODOMENU_CHILD_FRAME_ID;
+            type = Type::ODOMENU;
             break;
-        default:
-            navsatfix.frame_id = "GNSS";
+        case fpa::FpaOdomPayload::Which::UNSPECIFIED:
+            ok = false;
+            type = Type::UNSPECIFIED;
+            break;
+    }
+    stamp = FpaGpsTimeToTime(payload.gps_time);
+    if (stamp.IsZero()) {
+        ok = false;
+    }
+
+    if (!pose.SetFromFpaOdomPayload(payload)) {
+        ok = false;
+    }
+    if (!twist.SetFromFpaOdomPayload(payload)) {
+        ok = false;
+    }
+
+    // Reset fields if orientation is bad FIXME: why?
+    if ((std::fabs(pose.orientation.w()) < std::numeric_limits<double>::epsilon()) || pose.orientation.vec().isZero()) {
+        pose = PoseWithCovData();
+        twist = TwistWithCovData();
+    }
+
+    valid = ok;
+    return ok;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+bool TfData::SetFromFpaTfPayload(const fpa::FpaTfPayload& payload) {
+    bool ok = true;
+    if (payload.gps_time.week.valid && payload.gps_time.tow.valid) {
+        stamp.SetWnoTow({payload.gps_time.week.value, payload.gps_time.tow.value, time::WnoTow::Sys::GPS});
+    } else {
+        ok = false;
+    }
+
+    if (payload.frame_a[0] != '\0') {
+        frame_id = "FP_" + std::string(payload.frame_a);
+    } else {
+        ok = false;
+    }
+    if (payload.frame_a[0] != '\0') {
+        child_frame_id = "FP_" + std::string(payload.frame_b);
+    } else {
+        ok = false;
+    }
+
+    if (payload.translation.valid) {
+        translation = {payload.translation.values[0], payload.translation.values[1], payload.translation.values[2]};
+    } else {
+        ok = false;
+    }
+
+    if (payload.orientation.valid) {
+        rotation = {payload.orientation.values[0], payload.orientation.values[1], payload.orientation.values[2],
+                    payload.orientation.values[3]};
+    } else {
+        ok = false;
+    }
+
+    // Check if TF is valid FIXME: Why?
+    if ((std::fabs(rotation.w()) < std::numeric_limits<double>::epsilon()) || rotation.vec().isZero()) {
+        ok = false;
+    }
+
+    valid = ok;
+    return ok;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+JumpDetector::JumpDetector() {
+    curr_pos_.setZero();
+    curr_cov_.setZero();
+    prev_pos_.setZero();
+    prev_cov_.setZero();
+    pos_diff_.setZero();
+}
+
+bool JumpDetector::Check(const OdometryData& odometry_data) {
+    bool jump_detected = false;
+
+    prev_pos_ = curr_pos_;
+    prev_cov_ = curr_cov_;
+    prev_stamp_ = curr_stamp_;
+
+    if (!(prev_pos_.isZero() || prev_cov_.isZero())) {
+        pos_diff_ = (prev_pos_ - odometry_data.pose.position).cwiseAbs();
+        if ((pos_diff_[0] > prev_cov_(0, 0)) || (pos_diff_[1] > prev_cov_(1, 1)) || (pos_diff_[2] > prev_cov_(2, 2))) {
+            jump_detected = true;
+            warning_ = string::Sprintf(
+                "Position jump detected! The change in position is greater than the estimated covariances. "
+                "Position difference: [ %.4f, %.4f, %.4f ], Covariances: [ %.4f, %.4f, %.4f ]",
+                pos_diff_[0], pos_diff_[1], pos_diff_[2], prev_cov_(0, 0), prev_cov_(1, 1), prev_cov_(2, 2));
+        }
+    }
+
+    curr_pos_ = odometry_data.pose.position;
+    curr_cov_ = odometry_data.pose.cov;
+    curr_stamp_ = odometry_data.stamp;
+
+    return jump_detected;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+NmeaEpochData::NmeaEpochData(const fpsdk::common::parser::fpa::FpaEpoch epoch) /* clang-format off */ :
+    epoch_   { epoch }  // clang-format on
+{
+    cov_enu_.setZero();
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+NmeaEpochData NmeaEpochData::CompleteAndReset() {
+    // clang-format off
+    switch (epoch_) {
+        case fpa::FpaEpoch::UNSPECIFIED: break;
+        case fpa::FpaEpoch::GNSS1:       frame_id_ = GNSS_FRAME_ID; break;
+        case fpa::FpaEpoch::GNSS2:       frame_id_ = GNSS1_FRAME_ID; break;
+        case fpa::FpaEpoch::GNSS:        frame_id_ = GNSS2_FRAME_ID; break;
+        case fpa::FpaEpoch::FUSION:      frame_id_ = ODOMETRY_CHILD_FRAME_ID; break;
+    }
+    // clang-format on
+
+    // Complete GSA and GSV collection
+    gsa_gsv_.Complete();
+
+    // Full date and time is available in RMC or ZDA
+    if (rmc_.date.valid && rmc_.time.valid) {
+        stamp_.SetUtcTime(
+            {rmc_.date.years, rmc_.date.months, rmc_.date.days, rmc_.time.hours, rmc_.time.mins, rmc_.time.secs});
+    } else if (zda_.date.valid && zda_.time.valid) {
+        stamp_.SetUtcTime(
+            {zda_.date.years, zda_.date.months, zda_.date.days, zda_.time.hours, zda_.time.mins, zda_.time.secs});
+    }
+
+    // Use best available info...
+    // clang-format off
+    if      (rmc_.date.valid) { date_ = rmc_.date; }
+    else if (zda_.date.valid) { date_ = zda_.date; }
+    if      (gga_.time.valid) { time_ = gga_.time; }
+    else if (gll_.time.valid) { time_ = gll_.time; }
+    else if (gst_.time.valid) { time_ = gst_.time; }
+    else if (rmc_.time.valid) { time_ = rmc_.time; }
+    else if (zda_.time.valid) { time_ = zda_.time; }
+    if      (gga_.llh.latlon_valid) { llh_ = gga_.llh; }
+    else if (rmc_.llh.latlon_valid) { llh_ = rmc_.llh; }
+    else if (gll_.ll.latlon_valid)  { llh_ = gll_.ll; } // last as it does not have height
+    // clang-format on
+
+    status_ = (gll_.status > rmc_.status ? gll_.status : rmc_.status);
+    navstatus_ = rmc_.navstatus;
+    mode1_ = rmc_.mode;
+    mode2_ = (gll_.mode > vtg_.mode ? gll_.mode : vtg_.mode);
+    quality_ = gga_.quality;
+    opmode_ = gsa_.opmode;
+    navmode_ = gsa_.navmode;
+    num_sv_ = gga_.num_sv;
+    diff_age_ = gga_.diff_age;
+    diff_sta_ = gga_.diff_sta;
+    rms_range_ = gst_.rms_range;
+    std_major_ = gst_.std_major;
+    std_minor_ = gst_.std_minor;
+    angle_major_ = gst_.angle_major;
+    std_lat_ = gst_.std_lat;
+    std_lon_ = gst_.std_lon;
+    std_alt_ = gst_.std_alt;
+    pdop_ = gsa_.pdop;
+    hdop_ = (gsa_.hdop.valid ? gsa_.hdop : gga_.hdop);
+    vdop_ = gsa_.vdop;
+    heading_ = hdt_.heading;
+    local_hr_ = zda_.local_hr;
+    local_min_ = zda_.local_min;
+    speed_ = rmc_.speed;
+    course_ = rmc_.course;
+    cogt_ = vtg_.cogt;
+    cogm_ = vtg_.cogm;
+    sogn_ = vtg_.sogn;
+    sogk_ = vtg_.sogk;
+
+    if (std_lat_.valid && std_lon_.valid) {
+        cov_enu_(0, 0) = std_lon_.value * std_lon_.value;
+        cov_enu_(1, 1) = std_lat_.value * std_lat_.value;
+        cov_enu_(2, 2) = std_alt_.value * std_alt_.value;
+    } else if (hdop_.valid && vdop_.valid) {
+        cov_enu_(0, 0) = hdop_.value * hdop_.value;
+        cov_enu_(1, 1) = hdop_.value * hdop_.value;
+        cov_enu_(2, 2) = vdop_.value * vdop_.value;
+    } else if (pdop_.valid) {
+        cov_enu_(0, 0) = pdop_.value * pdop_.value;
+        cov_enu_(1, 1) = pdop_.value * pdop_.value;
+        cov_enu_(2, 2) = pdop_.value * pdop_.value * 4.0;
     }
+
+    // Return data to user and reset ourselves back to empty
+    NmeaEpochData data(epoch_);
+    std::swap(data, *this);
+    return data;
 }
 
-template <>
-void NovToData<BESTGNSSPOSMem, NavSatFixData>(const Oem7MessageHeaderMem* const header, const BESTGNSSPOSMem* const nov,
-                                              NavSatFixData& data) {
-    BestGnssPosToNavSatFix(header, nov, data);
+// ---------------------------------------------------------------------------------------------------------------------
+
+void HelloWorld() {
+    NOTICE("Fixposition driver (https://github.com/fixposition/fixposition_driver)");
+    NOTICE("Fixposition SDK %s", utils::GetVersionString());
+    NOTICE("%s", utils::GetCopyrightString());
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/corrimu.cpp b/fixposition_driver_lib/src/messages/fpa/corrimu.cpp
deleted file mode 100644
index 1ce1398..0000000
--- a/fixposition_driver_lib/src/messages/fpa/corrimu.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-CORRIMU parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int acc_x_idx = 5;
-static constexpr int acc_y_idx = 6;
-static constexpr int acc_z_idx = 7;
-static constexpr int rot_x_idx = 8;
-static constexpr int rot_y_idx = 9;
-static constexpr int rot_z_idx = 10;
-
-void FP_CORRIMU::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-CORRIMU string with " << tokens.size() << " fields!\n";
-
-    } else {
-        // If size is ok, check version
-        const int version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-CORRIMU string with version " << version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-    
-    // Populate fields
-    imu.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-    imu.linear_acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx));
-    imu.angular_velocity = Vector3ToEigen(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx));
-    imu.frame_id = "FP_VRTK";
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/eoe.cpp b/fixposition_driver_lib/src/messages/fpa/eoe.cpp
deleted file mode 100644
index e1bda7f..0000000
--- a/fixposition_driver_lib/src/messages/fpa/eoe.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-EOE parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int epoch_idx = 5;
-
-void FP_EOE::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-EOE string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-EOE string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Parse time
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-    // Populate fields
-    epoch = tokens.at(epoch_idx);
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/gnssant.cpp b/fixposition_driver_lib/src/messages/fpa/gnssant.cpp
deleted file mode 100644
index cb07c17..0000000
--- a/fixposition_driver_lib/src/messages/fpa/gnssant.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-GNSSANT parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int gnss1_state_idx = 5;
-static constexpr int gnss1_power_idx = 6;
-static constexpr int gnss1_age_idx = 7;
-static constexpr int gnss2_state_idx = 8;
-static constexpr int gnss2_power_idx = 9;
-static constexpr int gnss2_age_idx = 10;
-
-void FP_GNSSANT::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-GNSSANT string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-GNSSANT string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Populate VRTK message header
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-    // GNSS status data
-    gnss1_state = tokens.at(gnss1_state_idx);
-    gnss1_power = tokens.at(gnss1_power_idx);
-    gnss1_age   = StringToInt(tokens.at(gnss1_age_idx));
-    gnss2_state = tokens.at(gnss2_state_idx);
-    gnss2_power = tokens.at(gnss2_power_idx);
-    gnss2_age   = StringToInt(tokens.at(gnss2_age_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp b/fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp
deleted file mode 100644
index 1d8a06d..0000000
--- a/fixposition_driver_lib/src/messages/fpa/gnsscorr.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-GNSSCORR parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int gnss1_fix_idx = 5;
-static constexpr int gnss1_nsig_l1_idx = 6;
-static constexpr int gnss1_nsig_l2_idx = 7;
-static constexpr int gnss2_fix_idx = 8;
-static constexpr int gnss2_nsig_l1_idx = 9;
-static constexpr int gnss2_nsig_l2_idx = 10;
-static constexpr int corr_latency_idx = 11;
-static constexpr int corr_update_rate_idx = 12;
-static constexpr int corr_data_rate_idx = 13;
-static constexpr int corr_msg_rate_idx = 14;
-static constexpr int sta_id_idx = 15;
-static constexpr int sta_lat_idx = 16;
-static constexpr int sta_lon_idx = 17;
-static constexpr int sta_height_idx = 18;
-static constexpr int sta_dist_idx = 19;
-
-void FP_GNSSCORR::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-GNSSCORR string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-GNSSCORR string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Populate VRTK message header
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-    // GNSS status data
-    gnss1_fix     = ParseStatusFlag(tokens, gnss1_fix_idx);
-    gnss1_nsig_l1 = StringToInt(tokens.at(gnss1_nsig_l1_idx));
-    gnss1_nsig_l2 = StringToInt(tokens.at(gnss1_nsig_l2_idx));
-    gnss2_fix     = ParseStatusFlag(tokens, gnss2_fix_idx);
-    gnss2_nsig_l1 = StringToInt(tokens.at(gnss2_nsig_l1_idx));
-    gnss2_nsig_l2 = StringToInt(tokens.at(gnss2_nsig_l2_idx));
-
-    // Correction data status
-    corr_latency     = StringToDouble(tokens.at(corr_latency_idx));
-    corr_update_rate = StringToDouble(tokens.at(corr_update_rate_idx));
-    corr_data_rate   = StringToDouble(tokens.at(corr_data_rate_idx));
-    corr_msg_rate    = StringToDouble(tokens.at(corr_msg_rate_idx));
-    sta_id     = StringToInt(tokens.at(sta_id_idx));
-    sta_llh    = Vector3ToEigen(tokens.at(sta_lat_idx), tokens.at(sta_lon_idx), tokens.at(sta_height_idx));
-    sta_dist   = StringToInt(tokens.at(sta_dist_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/imubias.cpp b/fixposition_driver_lib/src/messages/fpa/imubias.cpp
deleted file mode 100644
index 66c4ad0..0000000
--- a/fixposition_driver_lib/src/messages/fpa/imubias.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-IMUBIAS parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int fusion_imu_idx = 5;
-static constexpr int imu_status_idx = 6;
-static constexpr int imu_noise_idx = 7;
-static constexpr int imu_conv_idx = 8;
-static constexpr int bias_acc_x_idx = 9;
-static constexpr int bias_acc_y_idx = 10;
-static constexpr int bias_acc_z_idx = 11;
-static constexpr int bias_gyr_x_idx = 12;
-static constexpr int bias_gyr_y_idx = 13;
-static constexpr int bias_gyr_z_idx = 14;
-static constexpr int bias_cov_acc_x_idx = 15;
-static constexpr int bias_cov_acc_y_idx = 16;
-static constexpr int bias_cov_acc_z_idx = 17;
-static constexpr int bias_cov_gyr_x_idx = 18;
-static constexpr int bias_cov_gyr_y_idx = 19;
-static constexpr int bias_cov_gyr_z_idx = 20;
-
-void FP_IMUBIAS::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-IMUBIAS string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-IMUBIAS string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Parse time
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-    // Populate fields
-    frame_id = "FP_VRTK";
-    fusion_imu = ParseStatusFlag(tokens, fusion_imu_idx);
-    imu_status = ParseStatusFlag(tokens, imu_status_idx);
-    imu_noise = ParseStatusFlag(tokens, imu_noise_idx);
-    imu_conv = ParseStatusFlag(tokens, imu_conv_idx);
-    bias_acc = Vector3ToEigen(tokens.at(bias_acc_x_idx), tokens.at(bias_acc_y_idx), tokens.at(bias_acc_z_idx));
-    bias_gyr = Vector3ToEigen(tokens.at(bias_gyr_x_idx), tokens.at(bias_gyr_y_idx), tokens.at(bias_gyr_z_idx));
-    bias_cov_acc = Vector3ToEigen(tokens.at(bias_cov_acc_x_idx), tokens.at(bias_cov_acc_y_idx), tokens.at(bias_cov_acc_z_idx));
-    bias_cov_gyr = Vector3ToEigen(tokens.at(bias_cov_gyr_x_idx), tokens.at(bias_cov_gyr_y_idx), tokens.at(bias_cov_gyr_z_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/llh.cpp b/fixposition_driver_lib/src/messages/fpa/llh.cpp
deleted file mode 100644
index 6766799..0000000
--- a/fixposition_driver_lib/src/messages/fpa/llh.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-LLH parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int latitude_idx = 5;
-static constexpr int longitude_idx = 6;
-static constexpr int height_idx = 7;
-static constexpr int pos_cov_ee_idx = 8;
-static constexpr int pos_cov_nn_idx = 9;
-static constexpr int pos_cov_uu_idx = 10;
-static constexpr int pos_cov_en_idx = 11;
-static constexpr int pos_cov_nu_idx = 12;
-static constexpr int pos_cov_eu_idx = 13;
-
-void FP_LLH::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-LLH string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-LLH string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Populate VRTK message header
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-    // LLH position
-    llh = Vector3ToEigen(tokens.at(latitude_idx), tokens.at(longitude_idx), tokens.at(height_idx));
-    
-    // Covariance
-    cov = BuildCovMat3D(StringToDouble(tokens.at(pos_cov_ee_idx)), StringToDouble(tokens.at(pos_cov_nn_idx)),
-                        StringToDouble(tokens.at(pos_cov_uu_idx)), StringToDouble(tokens.at(pos_cov_en_idx)),
-                        StringToDouble(tokens.at(pos_cov_nu_idx)), StringToDouble(tokens.at(pos_cov_eu_idx)));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/odomenu.cpp b/fixposition_driver_lib/src/messages/fpa/odomenu.cpp
deleted file mode 100644
index 8aa45c8..0000000
--- a/fixposition_driver_lib/src/messages/fpa/odomenu.cpp
+++ /dev/null
@@ -1,139 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-ODOMENU parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int pos_x_idx = 5;
-static constexpr int pos_y_idx = 6;
-static constexpr int pos_z_idx = 7;
-static constexpr int orientation_w_idx = 8;
-static constexpr int orientation_x_idx = 9;
-static constexpr int orientation_y_idx = 10;
-static constexpr int orientation_z_idx = 11;
-static constexpr int vel_x_idx = 12;
-static constexpr int vel_y_idx = 13;
-static constexpr int vel_z_idx = 14;
-static constexpr int rot_x_idx = 15;
-static constexpr int rot_y_idx = 16;
-static constexpr int rot_z_idx = 17;
-static constexpr int acc_x_idx = 18;
-static constexpr int acc_y_idx = 19;
-static constexpr int acc_z_idx = 20;
-static constexpr int fusion_status_idx = 21;
-static constexpr int imu_bias_status_idx = 22;
-static constexpr int gnss1_fix_type_idx = 23;
-static constexpr int gnss2_fix_type_idx = 24;
-static constexpr int wheelspeed_status_idx = 25;
-static constexpr int pos_cov_xx_idx = 26;
-static constexpr int pos_cov_yy_idx = 27;
-static constexpr int pos_cov_zz_idx = 28;
-static constexpr int pos_cov_xy_idx = 29;
-static constexpr int pos_cov_yz_idx = 30;
-static constexpr int pos_cov_xz_idx = 31;
-static constexpr int orientation_cov_xx_idx = 32;
-static constexpr int orientation_cov_yy_idx = 33;
-static constexpr int orientation_cov_zz_idx = 34;
-static constexpr int orientation_cov_xy_idx = 35;
-static constexpr int orientation_cov_yz_idx = 36;
-static constexpr int orientation_cov_xz_idx = 37;
-static constexpr int vel_cov_xx_idx = 38;
-static constexpr int vel_cov_yy_idx = 39;
-static constexpr int vel_cov_zz_idx = 40;
-static constexpr int vel_cov_xy_idx = 41;
-static constexpr int vel_cov_yz_idx = 42;
-static constexpr int vel_cov_xz_idx = 43;
-
-void FP_ODOMENU::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-ODOMENU string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-ODOMENU string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // VRTK status flags
-    fusion_status = ParseStatusFlag(tokens, fusion_status_idx);
-    imu_bias_status = ParseStatusFlag(tokens, imu_bias_status_idx);
-    gnss1_status = ParseStatusFlag(tokens, gnss1_fix_type_idx);
-    gnss2_status = ParseStatusFlag(tokens, gnss2_fix_type_idx);
-    wheelspeed_status = ParseStatusFlag(tokens, wheelspeed_status_idx);
-
-    if (fusion_status >= static_cast<int>(FusionStatus::INERTIAL_GNSS_FUSION)) {
-        // Message header
-        odom.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-        // Pose & Cov
-        odom.pose.position = Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx));
-        odom.pose.orientation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx),
-                                               tokens.at(orientation_y_idx), tokens.at(orientation_z_idx));
-
-        if (odom.pose.orientation.vec().isZero() && (odom.pose.orientation.w() == 0)) {
-            // Change Fusion status to invalid state
-            fusion_status = -1;
-
-            // Reset corresponding fields
-            odom.stamp = fixposition::times::GpsTime();
-            odom.frame_id = frame_id;
-            odom.child_frame_id = child_frame_id;
-            odom.pose = PoseWithCovData();
-            odom.twist = TwistWithCovData();
-            acceleration.setZero();
-            return;
-        }
-
-        odom.pose.cov = BuildCovMat6D(
-            StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)),
-            StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)),
-            StringToDouble(tokens.at(pos_cov_yz_idx)), StringToDouble(tokens.at(pos_cov_xz_idx)),
-            StringToDouble(tokens.at(orientation_cov_xx_idx)), StringToDouble(tokens.at(orientation_cov_yy_idx)),
-            StringToDouble(tokens.at(orientation_cov_zz_idx)), StringToDouble(tokens.at(orientation_cov_xy_idx)),
-            StringToDouble(tokens.at(orientation_cov_yz_idx)), StringToDouble(tokens.at(orientation_cov_xz_idx)));
-
-        // Twist & Cov
-        odom.twist.linear = Vector3ToEigen(tokens.at(vel_x_idx), tokens.at(vel_y_idx), tokens.at(vel_z_idx));
-        odom.twist.angular = Vector3ToEigen(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx));
-        odom.twist.cov = BuildCovMat6D(
-            StringToDouble(tokens.at(vel_cov_xx_idx)), StringToDouble(tokens.at(vel_cov_yy_idx)),
-            StringToDouble(tokens.at(vel_cov_zz_idx)), StringToDouble(tokens.at(vel_cov_xy_idx)),
-            StringToDouble(tokens.at(vel_cov_yz_idx)), StringToDouble(tokens.at(vel_cov_xz_idx)), 0, 0, 0, 0, 0, 0);
-
-        // Acceleration
-        acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx));
-    }
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/odometry.cpp b/fixposition_driver_lib/src/messages/fpa/odometry.cpp
deleted file mode 100644
index a86ce89..0000000
--- a/fixposition_driver_lib/src/messages/fpa/odometry.cpp
+++ /dev/null
@@ -1,141 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-ODOMETRY parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int pos_x_idx = 5;
-static constexpr int pos_y_idx = 6;
-static constexpr int pos_z_idx = 7;
-static constexpr int orientation_w_idx = 8;
-static constexpr int orientation_x_idx = 9;
-static constexpr int orientation_y_idx = 10;
-static constexpr int orientation_z_idx = 11;
-static constexpr int vel_x_idx = 12;
-static constexpr int vel_y_idx = 13;
-static constexpr int vel_z_idx = 14;
-static constexpr int rot_x_idx = 15;
-static constexpr int rot_y_idx = 16;
-static constexpr int rot_z_idx = 17;
-static constexpr int acc_x_idx = 18;
-static constexpr int acc_y_idx = 19;
-static constexpr int acc_z_idx = 20;
-static constexpr int fusion_status_idx = 21;
-static constexpr int imu_bias_status_idx = 22;
-static constexpr int gnss1_fix_type_idx = 23;
-static constexpr int gnss2_fix_type_idx = 24;
-static constexpr int wheelspeed_status_idx = 25;
-static constexpr int pos_cov_xx_idx = 26;
-static constexpr int pos_cov_yy_idx = 27;
-static constexpr int pos_cov_zz_idx = 28;
-static constexpr int pos_cov_xy_idx = 29;
-static constexpr int pos_cov_yz_idx = 30;
-static constexpr int pos_cov_xz_idx = 31;
-static constexpr int orientation_cov_xx_idx = 32;
-static constexpr int orientation_cov_yy_idx = 33;
-static constexpr int orientation_cov_zz_idx = 34;
-static constexpr int orientation_cov_xy_idx = 35;
-static constexpr int orientation_cov_yz_idx = 36;
-static constexpr int orientation_cov_xz_idx = 37;
-static constexpr int vel_cov_xx_idx = 38;
-static constexpr int vel_cov_yy_idx = 39;
-static constexpr int vel_cov_zz_idx = 40;
-static constexpr int vel_cov_xy_idx = 41;
-static constexpr int vel_cov_yz_idx = 42;
-static constexpr int vel_cov_xz_idx = 43;
-static constexpr int sw_version_idx = 44;
-
-void FP_ODOMETRY::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-ODOMETRY string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-ODOMETRY string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // VRTK status flags
-    fusion_status = ParseStatusFlag(tokens, fusion_status_idx);
-    imu_bias_status = ParseStatusFlag(tokens, imu_bias_status_idx);
-    gnss1_status = ParseStatusFlag(tokens, gnss1_fix_type_idx);
-    gnss2_status = ParseStatusFlag(tokens, gnss2_fix_type_idx);
-    wheelspeed_status = ParseStatusFlag(tokens, wheelspeed_status_idx);
-    version = tokens.at(sw_version_idx).empty() ? "UNKNOWN" : tokens.at(sw_version_idx);
-
-    if (fusion_status >= static_cast<int>(FusionStatus::INERTIAL_GNSS_FUSION)) {
-        // Message header
-        odom.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-        // Pose & Cov
-        odom.pose.position = Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx));
-        odom.pose.orientation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx),
-                                               tokens.at(orientation_y_idx), tokens.at(orientation_z_idx));
-
-        if (odom.pose.orientation.vec().isZero() && (odom.pose.orientation.w() == 0)) {
-            // Change Fusion status to invalid state
-            fusion_status = -1;
-
-            // Reset corresponding fields
-            odom.stamp = fixposition::times::GpsTime();
-            odom.frame_id = frame_id;
-            odom.child_frame_id = child_frame_id;
-            odom.pose = PoseWithCovData();
-            odom.twist = TwistWithCovData();
-            acceleration.setZero();
-            return;
-        }
-
-        odom.pose.cov = BuildCovMat6D(
-            StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)),
-            StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)),
-            StringToDouble(tokens.at(pos_cov_yz_idx)), StringToDouble(tokens.at(pos_cov_xz_idx)),
-            StringToDouble(tokens.at(orientation_cov_xx_idx)), StringToDouble(tokens.at(orientation_cov_yy_idx)),
-            StringToDouble(tokens.at(orientation_cov_zz_idx)), StringToDouble(tokens.at(orientation_cov_xy_idx)),
-            StringToDouble(tokens.at(orientation_cov_yz_idx)), StringToDouble(tokens.at(orientation_cov_xz_idx)));
-
-        // Twist & Cov
-        odom.twist.linear = Vector3ToEigen(tokens.at(vel_x_idx), tokens.at(vel_y_idx), tokens.at(vel_z_idx));
-        odom.twist.angular = Vector3ToEigen(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx));
-        odom.twist.cov = BuildCovMat6D(
-            StringToDouble(tokens.at(vel_cov_xx_idx)), StringToDouble(tokens.at(vel_cov_yy_idx)),
-            StringToDouble(tokens.at(vel_cov_zz_idx)), StringToDouble(tokens.at(vel_cov_xy_idx)),
-            StringToDouble(tokens.at(vel_cov_yz_idx)), StringToDouble(tokens.at(vel_cov_xz_idx)), 0, 0, 0, 0, 0, 0);
-
-        // Acceleration
-        acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx));
-    }
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/odomsh.cpp b/fixposition_driver_lib/src/messages/fpa/odomsh.cpp
deleted file mode 100644
index d3a151e..0000000
--- a/fixposition_driver_lib/src/messages/fpa/odomsh.cpp
+++ /dev/null
@@ -1,139 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-ODOMSH parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int pos_x_idx = 5;
-static constexpr int pos_y_idx = 6;
-static constexpr int pos_z_idx = 7;
-static constexpr int orientation_w_idx = 8;
-static constexpr int orientation_x_idx = 9;
-static constexpr int orientation_y_idx = 10;
-static constexpr int orientation_z_idx = 11;
-static constexpr int vel_x_idx = 12;
-static constexpr int vel_y_idx = 13;
-static constexpr int vel_z_idx = 14;
-static constexpr int rot_x_idx = 15;
-static constexpr int rot_y_idx = 16;
-static constexpr int rot_z_idx = 17;
-static constexpr int acc_x_idx = 18;
-static constexpr int acc_y_idx = 19;
-static constexpr int acc_z_idx = 20;
-static constexpr int fusion_status_idx = 21;
-static constexpr int imu_bias_status_idx = 22;
-static constexpr int gnss1_fix_type_idx = 23;
-static constexpr int gnss2_fix_type_idx = 24;
-static constexpr int wheelspeed_status_idx = 25;
-static constexpr int pos_cov_xx_idx = 26;
-static constexpr int pos_cov_yy_idx = 27;
-static constexpr int pos_cov_zz_idx = 28;
-static constexpr int pos_cov_xy_idx = 29;
-static constexpr int pos_cov_yz_idx = 30;
-static constexpr int pos_cov_xz_idx = 31;
-static constexpr int orientation_cov_xx_idx = 32;
-static constexpr int orientation_cov_yy_idx = 33;
-static constexpr int orientation_cov_zz_idx = 34;
-static constexpr int orientation_cov_xy_idx = 35;
-static constexpr int orientation_cov_yz_idx = 36;
-static constexpr int orientation_cov_xz_idx = 37;
-static constexpr int vel_cov_xx_idx = 38;
-static constexpr int vel_cov_yy_idx = 39;
-static constexpr int vel_cov_zz_idx = 40;
-static constexpr int vel_cov_xy_idx = 41;
-static constexpr int vel_cov_yz_idx = 42;
-static constexpr int vel_cov_xz_idx = 43;
-
-void FP_ODOMSH::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-ODOMSH string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-ODOMSH string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // VRTK status flags
-    fusion_status = ParseStatusFlag(tokens, fusion_status_idx);
-    imu_bias_status = ParseStatusFlag(tokens, imu_bias_status_idx);
-    gnss1_status = ParseStatusFlag(tokens, gnss1_fix_type_idx);
-    gnss2_status = ParseStatusFlag(tokens, gnss2_fix_type_idx);
-    wheelspeed_status = ParseStatusFlag(tokens, wheelspeed_status_idx);
-
-    if (fusion_status >= static_cast<int>(FusionStatus::INERTIAL_GNSS_FUSION)) {
-        // Message header
-        odom.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-        // Pose & Cov
-        odom.pose.position = Vector3ToEigen(tokens.at(pos_x_idx), tokens.at(pos_y_idx), tokens.at(pos_z_idx));
-        odom.pose.orientation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx),
-                                               tokens.at(orientation_y_idx), tokens.at(orientation_z_idx));
-
-        if (odom.pose.orientation.vec().isZero() && (odom.pose.orientation.w() == 0)) {
-            // Change Fusion status to invalid state
-            fusion_status = -1;
-
-            // Reset corresponding fields
-            odom.stamp = fixposition::times::GpsTime();
-            odom.frame_id = frame_id;
-            odom.child_frame_id = child_frame_id;
-            odom.pose = PoseWithCovData();
-            odom.twist = TwistWithCovData();
-            acceleration.setZero();
-            return;
-        }
-
-        odom.pose.cov = BuildCovMat6D(
-            StringToDouble(tokens.at(pos_cov_xx_idx)), StringToDouble(tokens.at(pos_cov_yy_idx)),
-            StringToDouble(tokens.at(pos_cov_zz_idx)), StringToDouble(tokens.at(pos_cov_xy_idx)),
-            StringToDouble(tokens.at(pos_cov_yz_idx)), StringToDouble(tokens.at(pos_cov_xz_idx)),
-            StringToDouble(tokens.at(orientation_cov_xx_idx)), StringToDouble(tokens.at(orientation_cov_yy_idx)),
-            StringToDouble(tokens.at(orientation_cov_zz_idx)), StringToDouble(tokens.at(orientation_cov_xy_idx)),
-            StringToDouble(tokens.at(orientation_cov_yz_idx)), StringToDouble(tokens.at(orientation_cov_xz_idx)));
-
-        // Twist & Cov
-        odom.twist.linear = Vector3ToEigen(tokens.at(vel_x_idx), tokens.at(vel_y_idx), tokens.at(vel_z_idx));
-        odom.twist.angular = Vector3ToEigen(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx));
-        odom.twist.cov = BuildCovMat6D(
-            StringToDouble(tokens.at(vel_cov_xx_idx)), StringToDouble(tokens.at(vel_cov_yy_idx)),
-            StringToDouble(tokens.at(vel_cov_zz_idx)), StringToDouble(tokens.at(vel_cov_xy_idx)),
-            StringToDouble(tokens.at(vel_cov_yz_idx)), StringToDouble(tokens.at(vel_cov_xz_idx)), 0, 0, 0, 0, 0, 0);
-
-        // Acceleration
-        acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx));
-    }
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/odomstatus.cpp b/fixposition_driver_lib/src/messages/fpa/odomstatus.cpp
deleted file mode 100644
index 0baa931..0000000
--- a/fixposition_driver_lib/src/messages/fpa/odomstatus.cpp
+++ /dev/null
@@ -1,111 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-ODOMSTATUS parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int init_status_idx = 5;
-static constexpr int fusion_imu_idx = 6;
-static constexpr int fusion_gnss1_idx = 7;
-static constexpr int fusion_gnss2_idx = 8;
-static constexpr int fusion_corr_idx = 9;
-static constexpr int fusion_cam1_idx = 10;
-static constexpr int reserved0_idx = 11;
-static constexpr int fusion_ws_idx = 12;
-static constexpr int fusion_markers_idx = 13;
-static constexpr int reserved1_idx = 14;
-static constexpr int reserved2_idx = 15;
-static constexpr int reserved3_idx = 16;
-static constexpr int reserved4_idx = 17;
-static constexpr int imu_status_idx = 18;
-static constexpr int imu_noise_idx = 19;
-static constexpr int imu_conv_idx = 20;
-static constexpr int gnss1_status_idx = 21;
-static constexpr int gnss2_status_idx = 22;
-static constexpr int baseline_status_idx = 23;
-static constexpr int corr_status_idx = 24;
-static constexpr int cam1_status_idx = 25;
-static constexpr int reserved5_idx = 26;
-static constexpr int ws_status_idx = 27;
-static constexpr int ws_conv_idx = 28;
-static constexpr int markers_status_idx = 29;
-static constexpr int markers_conv_idx = 30;
-static constexpr int reserved6_idx = 31;
-static constexpr int reserved7_idx = 32;
-static constexpr int reserved8_idx = 33;
-static constexpr int reserved9_idx = 34;
-static constexpr int reserved10_idx = 35;
-static constexpr int reserved11_idx = 36;
-static constexpr int reserved12_idx = 37;
-static constexpr int reserved13_idx = 38;
-static constexpr int reserved14_idx = 39;
-static constexpr int reserved15_idx = 40;
-
-void FP_ODOMSTATUS::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-ODOMSTATUS string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-ODOMSTATUS string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Parse time
-    stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-
-    // Populate fields
-    init_status = ParseStatusFlag(tokens, init_status_idx);
-    fusion_imu = ParseStatusFlag(tokens, fusion_imu_idx);
-    fusion_gnss1 = ParseStatusFlag(tokens, fusion_gnss1_idx);
-    fusion_gnss2 = ParseStatusFlag(tokens, fusion_gnss2_idx);
-    fusion_corr = ParseStatusFlag(tokens, fusion_corr_idx);
-    fusion_cam1 = ParseStatusFlag(tokens, fusion_cam1_idx);
-    fusion_ws = ParseStatusFlag(tokens, fusion_ws_idx);
-    fusion_markers = ParseStatusFlag(tokens, fusion_markers_idx);
-    imu_status = ParseStatusFlag(tokens, imu_status_idx);
-    imu_noise = ParseStatusFlag(tokens, imu_noise_idx);
-    imu_conv = ParseStatusFlag(tokens, imu_conv_idx);
-    gnss1_status = ParseStatusFlag(tokens, gnss1_status_idx);
-    gnss2_status = ParseStatusFlag(tokens, gnss2_status_idx);
-    baseline_status = ParseStatusFlag(tokens, baseline_status_idx);
-    corr_status = ParseStatusFlag(tokens, corr_status_idx);
-    cam1_status = ParseStatusFlag(tokens, cam1_status_idx);
-    ws_status = ParseStatusFlag(tokens, ws_status_idx);
-    ws_conv = ParseStatusFlag(tokens, ws_conv_idx);
-    markers_status = ParseStatusFlag(tokens, markers_status_idx);
-    markers_conv = ParseStatusFlag(tokens, markers_conv_idx);
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/rawimu.cpp b/fixposition_driver_lib/src/messages/fpa/rawimu.cpp
deleted file mode 100644
index 8d324c1..0000000
--- a/fixposition_driver_lib/src/messages/fpa/rawimu.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-RAWIMU parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int acc_x_idx = 5;
-static constexpr int acc_y_idx = 6;
-static constexpr int acc_z_idx = 7;
-static constexpr int rot_x_idx = 8;
-static constexpr int rot_y_idx = 9;
-static constexpr int rot_z_idx = 10;
-
-void FP_RAWIMU::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-RAWIMU string with " << tokens.size() << " fields!\n";
-
-    } else {
-        // If size is ok, check version
-        const int version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-RAWIMU string with version " << version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-    
-    // Populate fields
-    imu.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-    imu.linear_acceleration = Vector3ToEigen(tokens.at(acc_x_idx), tokens.at(acc_y_idx), tokens.at(acc_z_idx));
-    imu.angular_velocity = Vector3ToEigen(tokens.at(rot_x_idx), tokens.at(rot_y_idx), tokens.at(rot_z_idx));
-    imu.frame_id = "FP_VRTK";
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/text.cpp b/fixposition_driver_lib/src/messages/fpa/text.cpp
deleted file mode 100644
index e050e98..0000000
--- a/fixposition_driver_lib/src/messages/fpa/text.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-TEXT parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int level_idx = 3;
-static constexpr int text_idx = 4;
-
-void FP_TEXT::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-TEXT string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-TEXT string with version " << version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Populate fields
-    level = tokens.at(level_idx);
-    text  = tokens.at(text_idx);
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/tf.cpp b/fixposition_driver_lib/src/messages/fpa/tf.cpp
deleted file mode 100644
index d0bb1e2..0000000
--- a/fixposition_driver_lib/src/messages/fpa/tf.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-TF parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int gps_week_idx = 3;
-static constexpr int gps_tow_idx = 4;
-static constexpr int from_frame_idx = 5;
-static constexpr int to_frame_idx = 6;
-static constexpr int translation_x_idx = 7;
-static constexpr int translation_y_idx = 8;
-static constexpr int translation_z_idx = 9;
-static constexpr int orientation_w_idx = 10;
-static constexpr int orientation_x_idx = 11;
-static constexpr int orientation_y_idx = 12;
-static constexpr int orientation_z_idx = 13;
-
-void FP_TF::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-TF string with " << tokens.size() << " fields!\n";
-
-    } else {
-        // If size is ok, check version
-        const int version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-TF string with version " << version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Populate fields
-    tf.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
-    tf.frame_id = "FP_" + tokens.at(from_frame_idx);
-    tf.child_frame_id = "FP_" + tokens.at(to_frame_idx);
-
-    tf.translation = Vector3ToEigen(tokens.at(translation_x_idx), 
-                                    tokens.at(translation_y_idx),
-                                    tokens.at(translation_z_idx));
-    tf.rotation = Vector4ToEigen(tokens.at(orientation_w_idx), tokens.at(orientation_x_idx),
-                                 tokens.at(orientation_y_idx), tokens.at(orientation_z_idx));
-
-    // Check if TF is valid
-    valid_tf = !(tf.rotation.w() == 0 && tf.rotation.vec().isZero());
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/fpa/tp.cpp b/fixposition_driver_lib/src/messages/fpa/tp.cpp
deleted file mode 100644
index af29cfd..0000000
--- a/fixposition_driver_lib/src/messages/fpa/tp.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of FP_A-TP parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/fpa_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int msg_type_idx = 1;
-static constexpr int msg_version_idx = 2;
-static constexpr int tp_name_idx = 3;
-static constexpr int timebase_idx = 4;
-static constexpr int timeref_idx = 5;
-static constexpr int tp_tow_sec_idx = 6;
-static constexpr int tp_tow_psec_idx = 7;
-static constexpr int gps_leaps_idx = 8;
-
-void FP_TP::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        // Size is wrong
-        std::cout << "Error in parsing FP_A-TP string with " << tokens.size() << " fields!\n";
-    } else {
-        // If size is ok, check version
-        const int _version = std::stoi(tokens.at(msg_version_idx));
-
-        ok = _version == kVersion_;
-        if (!ok) {
-            // Version is wrong
-            std::cout << "Error in parsing FP_A-TP string with version " << _version << "!\n";
-        }
-    }
-
-    if (!ok) {
-        // Reset message and return
-        ResetData();
-        return;
-    }
-
-    // Populate fields
-    tp_name = tokens.at(tp_name_idx);
-    timebase = tokens.at(timebase_idx);
-    timeref = tokens.at(timeref_idx);
-    tp_tow_sec = StringToInt(tokens.at(tp_tow_sec_idx));
-    tp_tow_psec = StringToDouble(tokens.at(tp_tow_psec_idx));
-    gps_leaps = StringToInt(tokens.at(gps_leaps_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gngsa.cpp b/fixposition_driver_lib/src/messages/nmea/gngsa.cpp
deleted file mode 100644
index ad69ce4..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gngsa.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GN-GSA parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int mode_op_idx = 1;
-static constexpr int mode_nav_idx = 2;
-static constexpr int ids_idx = 3;
-static constexpr int pdop_idx = 15;
-static constexpr int hdop_idx = 16;
-static constexpr int vdop_idx = 17;
-static constexpr int gnss_id_idx = 18;
-
-void GN_GSA::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GN-GSA string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-    
-    // Populate mode
-    mode_op = StringToChar(tokens.at(mode_op_idx));
-    mode_nav = StringToInt(tokens.at(mode_nav_idx));
-
-    // Populate ID numbers of satellites used
-    ids.clear();
-    for (unsigned int offset = 0; offset < 11; offset++) {
-        int value = StringToInt(tokens.at(ids_idx + offset));
-        
-        if (value != 0) {
-            ids.push_back(value);
-        } else {
-            break;
-        }
-    }
-
-    // Populate Dilution of Precision (DOP)
-    pdop = StringToDouble(tokens.at(pdop_idx));
-    hdop = StringToDouble(tokens.at(hdop_idx));
-    vdop = StringToDouble(tokens.at(vdop_idx));
-
-    // Populate GNSS ID
-    gnss_id = StringToInt(tokens.at(gnss_id_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gpgga.cpp b/fixposition_driver_lib/src/messages/nmea/gpgga.cpp
deleted file mode 100644
index 242ad10..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gpgga.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GP-GGA parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int time_idx = 1;
-static constexpr int lat_idx = 2;
-static constexpr int lat_ns_idx = 3;
-static constexpr int lon_idx = 4;
-static constexpr int lon_ew_idx = 5;
-static constexpr int quality_idx = 6;
-static constexpr int num_sv_idx = 7;
-static constexpr int hdop_idx = 8;
-static constexpr int alt_idx = 9;
-static constexpr int alt_unit_idx = 10;
-static constexpr int sep_idx = 11;
-static constexpr int sep_unit_idx = 12;
-static constexpr int diff_age_idx = 13;
-static constexpr int diff_sta_idx = 14;
-
-std::string join(const std::vector<std::string>& elements, const char& delimiter) {
-    std::ostringstream os;
-    for (auto it = elements.begin(); it != elements.end(); ++it) {
-        if (it != elements.begin()) {
-            os << delimiter;
-        }
-        os << *it;
-    }
-    return os.str();
-}
-
-void GP_GGA::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GP-GGA string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-
-    // Time string
-    time_str = tokens.at(time_idx);
-
-    // LLH coordinates
-    double _lat = 0.0;
-    double _lon = 0.0;
-    const std::string _latstr = tokens.at(lat_idx);
-    const std::string _lonstr = tokens.at(lon_idx);
-
-    if (!_latstr.empty()) {
-        _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60;
-        if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1;
-    }
-
-    if (!_lonstr.empty()) {
-        _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60;
-        if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1;
-    }
-
-    llh = Eigen::Vector3d(_lat, _lon, StringToDouble(tokens.at(alt_idx)));
-
-    // LLH indicators
-    lat_ns   = StringToChar(tokens.at(lat_ns_idx));
-    lon_ew   = StringToChar(tokens.at(lon_ew_idx));
-    alt_unit = StringToChar(tokens.at(alt_unit_idx));
-    quality  = ParseStatusFlag(tokens, quality_idx);
-    num_sv   = StringToInt(tokens.at(num_sv_idx));
-
-    // Dilution of precision
-    hdop = StringToDouble(tokens.at(hdop_idx));
-
-    // Differental data
-    diff_age = StringToDouble(tokens.at(diff_age_idx));
-    diff_sta = tokens.at(diff_sta_idx);
-
-    // ASCII sentence
-    sentence = join(tokens, ',');
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gpgll.cpp b/fixposition_driver_lib/src/messages/nmea/gpgll.cpp
deleted file mode 100644
index 9adcb3b..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gpgll.cpp
+++ /dev/null
@@ -1,67 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GP-GLL parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int lat_idx = 1;
-static constexpr int lat_ns_idx = 2;
-static constexpr int lon_idx = 3;
-static constexpr int lon_ew_idx = 4;
-static constexpr int time_idx = 5;
-static constexpr int status_idx = 6;
-static constexpr int mode_idx = 7;
-
-void GP_GLL::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GP-GLL string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-
-    // Populate time field
-    time_str = tokens.at(time_idx);
-
-    // LLH coordinates
-    double _lat = 0.0;
-    double _lon = 0.0;
-    const std::string _latstr = tokens.at(lat_idx);
-    const std::string _lonstr = tokens.at(lon_idx);
-
-    if (!_latstr.empty()) {
-        _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60;
-        if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1;
-    }
-
-    if (!_lonstr.empty()) {
-        _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60;
-        if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1;
-    }
-
-    latlon = Eigen::Vector2d(_lat, _lon);
-
-    // LLH indicators
-    status = StringToChar(tokens.at(status_idx));
-    lat_ns = StringToChar(tokens.at(lat_ns_idx));
-    lon_ew = StringToChar(tokens.at(lon_ew_idx));
-    mode   = StringToChar(tokens.at(mode_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gpgst.cpp b/fixposition_driver_lib/src/messages/nmea/gpgst.cpp
deleted file mode 100644
index cdc6dd5..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gpgst.cpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GP-GST parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int time_idx = 1;
-static constexpr int rms_range_idx = 2;
-static constexpr int std_major_idx = 3;
-static constexpr int std_minor_idx = 4;
-static constexpr int angle_major_idx = 5;
-static constexpr int std_lat_idx = 6;
-static constexpr int std_lon_idx = 7;
-static constexpr int std_alt_idx = 8;
-
-void GP_GST::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GP-GST string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-
-    // Populate time field
-    time_str = tokens.at(time_idx);
-
-    // Standard deviation of error ellipse
-    rms_range = StringToDouble(tokens.at(rms_range_idx));
-    std_major = StringToDouble(tokens.at(std_major_idx));
-    std_minor = StringToDouble(tokens.at(std_minor_idx));
-    angle_major = StringToDouble(tokens.at(angle_major_idx));
-
-    // Standard deviation of LLH coordinates
-    std_lat = StringToDouble(tokens.at(std_lat_idx));
-    std_lon = StringToDouble(tokens.at(std_lon_idx));
-    std_alt = StringToDouble(tokens.at(std_alt_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gphdt.cpp b/fixposition_driver_lib/src/messages/nmea/gphdt.cpp
deleted file mode 100644
index 1f8bc91..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gphdt.cpp
+++ /dev/null
@@ -1,39 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GP-HDT parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int heading_idx = 1;
-static constexpr int true_ind_idx = 2;
-
-void GP_HDT::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GP-HDT string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-
-    // Populate heading
-    heading = StringToDouble(tokens.at(heading_idx));
-    true_ind = StringToChar(tokens.at(true_ind_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gprmc.cpp b/fixposition_driver_lib/src/messages/nmea/gprmc.cpp
deleted file mode 100644
index b119b03..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gprmc.cpp
+++ /dev/null
@@ -1,81 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GP-RMC parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-// unit conversion constant
-static constexpr double knots_to_ms = 1852.0 / 3600.0; //!< convert knots to m/s
-
-/// msg field indices
-static constexpr int time_idx = 1;
-static constexpr int status_idx = 2;
-static constexpr int lat_idx = 3;
-static constexpr int lat_ns_idx = 4;
-static constexpr int lon_idx = 5;
-static constexpr int lon_ew_idx = 6;
-static constexpr int speed_idx = 7;
-static constexpr int course_idx = 8;
-static constexpr int date_idx = 9;
-static constexpr int magvar_idx = 10;
-static constexpr int magvar_ew_idx = 11;
-static constexpr int mode_idx = 12;
-
-void GP_RMC::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GP-RMC string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-
-    // Time and date strings
-    date_str = tokens.at(date_idx);
-    time_str = tokens.at(time_idx);
-
-    // LLH coordinates
-    double _lat = 0.0;
-    double _lon = 0.0;
-    const std::string _latstr = tokens.at(lat_idx);
-    const std::string _lonstr = tokens.at(lon_idx);
-
-    if (!_latstr.empty()) {
-        _lat = StringToDouble(_latstr.substr(0,2)) + StringToDouble((_latstr.substr(2))) / 60;
-        if (tokens.at(lat_ns_idx).compare("S") == 0) _lat *= -1;
-    }
-
-    if (!_lonstr.empty()) {
-        _lon = StringToDouble(_lonstr.substr(0,3)) + StringToDouble((_lonstr.substr(3))) / 60;
-        if (tokens.at(lon_ew_idx).compare("W") == 0) _lon *= -1;
-    }
-
-    latlon = Eigen::Vector2d(_lat, _lon);
-
-    // LLH indicators
-    status = StringToChar(tokens.at(status_idx));
-    lat_ns = StringToChar(tokens.at(lat_ns_idx));
-    lon_ew = StringToChar(tokens.at(lon_ew_idx));
-    mode   = StringToChar(tokens.at(mode_idx));
-
-    // Speed and course over ground
-    speed    = StringToDouble(tokens.at(speed_idx));
-    speed_ms = speed * knots_to_ms;
-    course   = StringToDouble(tokens.at(course_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gpvtg.cpp b/fixposition_driver_lib/src/messages/nmea/gpvtg.cpp
deleted file mode 100644
index c7b296e..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gpvtg.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GP-VTG parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int cog_true_idx = 1;
-static constexpr int cog_ref_t_idx = 2;
-static constexpr int cog_mag_idx = 3;
-static constexpr int cog_ref_m_idx = 4;
-static constexpr int sog_knot_idx = 5;
-static constexpr int sog_unit_n_idx = 6;
-static constexpr int sog_kph_idx = 7;
-static constexpr int sog_unit_k_idx = 8;
-static constexpr int mode_idx = 9;
-
-void GP_VTG::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GP-VTG string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-
-    // Populate COG
-    cog_true = StringToDouble(tokens.at(cog_true_idx));
-    cog_ref_t = StringToChar(tokens.at(cog_ref_t_idx));
-    cog_mag = StringToDouble(tokens.at(cog_mag_idx));
-    cog_ref_m = StringToChar(tokens.at(cog_ref_m_idx));
-
-    // Populate SOG
-    sog_knot = StringToDouble(tokens.at(sog_knot_idx));
-    sog_unit_n = StringToChar(tokens.at(sog_unit_n_idx));
-    sog_kph = StringToDouble(tokens.at(sog_kph_idx));
-    sog_unit_k = StringToChar(tokens.at(sog_unit_k_idx));
-
-    // Populate mode
-    mode = StringToChar(tokens.at(mode_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gpzda.cpp b/fixposition_driver_lib/src/messages/nmea/gpzda.cpp
deleted file mode 100644
index ed65ee0..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gpzda.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GP-ZDA parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int time_idx = 1;
-static constexpr int day_idx = 2;
-static constexpr int month_idx = 3;
-static constexpr int year_idx = 4;
-static constexpr int local_hr_idx = 5;
-static constexpr int local_min_idx = 6;
-
-void GP_ZDA::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Check if message size is wrong
-    bool ok = tokens.size() == kSize_;
-    if (!ok) {
-        std::cout << "Error in parsing NMEA-GP-ZDA string with " << tokens.size() << " fields!\n";
-        ResetData();
-        return;
-    }
-
-    // Populate time field
-    time_str = tokens.at(time_idx);
-
-    // Populate date field
-    std::string day = tokens.at(day_idx);
-    std::string month = tokens.at(month_idx);
-    std::string year = tokens.at(year_idx);
-    if (!day.empty() && !month.empty() && !year.empty()) {
-        date_str = day + '/' + month + '/' + year;
-    }
-
-    // Generate GPS timestamp
-    if (!time_str.empty() && !date_str.empty()) {
-        std::string utcTimeString = date_str + " " + time_str.substr(0,2) + ":" + time_str.substr(2,2) + ":" + time_str.substr(4);
-        std::string gps_tow, gps_week;
-        times::convertToGPSTime(utcTimeString, gps_week, gps_tow);
-        stamp = ConvertGpsTime(gps_week, gps_tow);
-    }
-
-    // Get local time
-    local_hr  = StringToInt(tokens.at(local_hr_idx));
-    local_min = StringToInt(tokens.at(local_min_idx));
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp b/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp
deleted file mode 100644
index b80c2c4..0000000
--- a/fixposition_driver_lib/src/messages/nmea/gxgsv.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GX-GSV parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-/// msg field indices
-static constexpr int sentences_idx = 1;
-static constexpr int sent_num_idx = 2;
-static constexpr int num_sats_idx = 3;
-static constexpr int sat_id_idx = 4;
-static constexpr int elev_idx = 5;
-static constexpr int azim_idx = 6;
-static constexpr int cno_idx = 7;
-//static constexpr int signal_id_idx = 4 + (4*num_sats);
-
-void GX_GSV::ConvertFromTokens(const std::vector<std::string>& tokens) {
-    // Reset vectors
-    ResetData();
-    
-    // Populate fields
-    sentences = StringToInt(tokens.at(sentences_idx));
-    sent_num = StringToInt(tokens.at(sent_num_idx));
-    num_sats = StringToInt(tokens.at(num_sats_idx));
-
-    // Populate satellite information
-    unsigned int offset = 0;
-    for (unsigned int i = 3; i < (tokens.size() - 2); i+=4) {
-        sat_id.push_back(StringToInt(tokens.at(sat_id_idx + 4*offset)));
-        elev.push_back(StringToInt(tokens.at(elev_idx + 4*offset)));
-        azim.push_back(StringToInt(tokens.at(azim_idx + 4*offset)));
-        cno.push_back(StringToInt(tokens.at(cno_idx + 4*offset)));
-        offset++;
-    }
-
-    // Obtain signal ID and type
-    std::string constellation = tokens.front();
-    signal_id = tokens.back();
-
-    if (constellation.size() >= 2) {
-        type = string2enum(constellation.substr(0,2));
-    } else {
-        type = SignalType::Invalid;
-    }
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/messages/nmea/nmea.cpp b/fixposition_driver_lib/src/messages/nmea/nmea.cpp
deleted file mode 100644
index f56d2f9..0000000
--- a/fixposition_driver_lib/src/messages/nmea/nmea.cpp
+++ /dev/null
@@ -1,155 +0,0 @@
-/**
- *  @file
- *  @brief Implementation of NMEA-GX-GSV parser
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/messages/nmea_type.hpp>
-#include <fixposition_driver_lib/messages/base_converter.hpp>
-
-namespace fixposition {
-
-void NmeaMessage::AddNmeaEpoch(const GP_GGA& msg) {
-    // Populate time if empty
-    if (time_str == "") {
-        time_str = msg.time_str;
-    }
-    gpgga_time_str = msg.time_str;
-    
-    // Populate LLH position
-    llh = msg.llh;
-
-    // Populate quality
-    quality = msg.quality;
-
-    // Populate total number of satellites
-    num_sv = msg.num_sv;
-
-    // Populate GNSS receiver HDOP
-    hdop_receiver = msg.hdop;
-
-    // Populate differential data information
-    diff_age = msg.diff_age;
-    diff_sta = msg.diff_sta;
-    
-    // Populate position covariance [m^2]
-    cov(0, 0) = msg.hdop * msg.hdop;
-    cov(1, 1) = msg.hdop * msg.hdop;
-    cov(2, 2) = 4 * msg.hdop * msg.hdop;
-    cov(0, 1) = cov(1, 0) = 0.0;
-    cov(0, 2) = cov(2, 0) = 0.0;
-    cov(1, 2) = cov(2, 1) = 0.0;
-}
-
-void NmeaMessage::AddNmeaEpoch(const GP_GLL& msg) {
-    // Populate time if empty
-    if (time_str == "") {
-        time_str = msg.time_str;
-    }
-    
-    // Populate latitude and longitude if the vector is empty
-    if (llh.isZero()) {
-        llh(0) = msg.latlon(0);
-        llh(1) = msg.latlon(1);
-    }
-}
-
-void NmeaMessage::AddNmeaEpoch(const GN_GSA& msg) {
-    // Populate DOP values (priority)
-    pdop = msg.pdop;
-    hdop = msg.hdop;
-    vdop = msg.vdop;
-
-    // Populate satellite information (priority)
-    ids = msg.ids;
-}
-
-void NmeaMessage::AddNmeaEpoch(const GP_GST& msg) {
-    // Populate time if empty
-    if (time_str == "") {
-        time_str = msg.time_str;
-    }
-
-    // Populate GNSS pseudorange error statistics (priority)
-    rms_range = msg.rms_range;
-    std_major = msg.std_major;
-    std_minor = msg.std_minor;
-    angle_major = msg.angle_major;
-    std_lat = msg.std_lat;
-    std_lon = msg.std_lon;
-    std_alt = msg.std_alt;
-}
-
-void NmeaMessage::AddNmeaEpoch(const GX_GSV& msg) {    
-    // Populate GNSS signal stats
-    for (u_int8_t i = 0; i < msg.sat_id.size(); i++) {
-        GnssSignalStats *stats = &gnss_signals[msg.type][msg.sat_id.at(i)];
-
-        // Populate necessary fields
-        if (stats->elev == 0) { stats->elev = msg.elev.at(i); }
-        if (stats->azim == 0) { stats->azim = msg.azim.at(i); }
-
-        // Populate CNO
-        if (msg.signal_id == "1" || msg.signal_id == "7") {
-            stats->cno_l1 = msg.cno.at(i);
-        } else if (msg.signal_id == "6" || msg.signal_id == "2" || msg.signal_id == "B" || msg.signal_id == "3") {
-            stats->cno_l2 = msg.cno.at(i);
-        }
-    }
-}
-
-void NmeaMessage::AddNmeaEpoch(const GP_HDT& msg) {
-    // Populate true heading (priority)
-    heading = msg.heading;
-}
-
-void NmeaMessage::AddNmeaEpoch(const GP_VTG& msg) {
-    // Populate SOG and COG if empty
-    if (speed == 0.0) {
-        speed = msg.sog_kph / 3.6; // Convert km/h to m/s
-    }
-
-    if (course == 0.0) {
-        course = msg.cog_true;
-    }
-}
-
-void NmeaMessage::AddNmeaEpoch(const GP_RMC& msg) {
-    // Populate time if empty
-    if (time_str == "") {
-        time_str = msg.time_str;
-    }
-
-    // Populate time if empty
-    if (date_str == "") {
-        date_str = msg.date_str;
-    }
-
-    // Populate SOG and COG (priority)
-    speed = msg.speed_ms;
-    course = msg.course;
-    
-    // Populate latitude and longitude if the vector is empty
-    if (llh.isZero()) {
-        llh(0) = msg.latlon(0);
-        llh(1) = msg.latlon(1);
-    }
-}
-
-void NmeaMessage::AddNmeaEpoch(const GP_ZDA& msg) {
-    // Populate time and date
-    time_str = msg.time_str;
-    date_str = msg.date_str;
-    gpzda_time_str = msg.time_str;
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/params.cpp b/fixposition_driver_lib/src/params.cpp
new file mode 100644
index 0000000..158d55a
--- /dev/null
+++ b/fixposition_driver_lib/src/params.cpp
@@ -0,0 +1,46 @@
+/**
+ * \verbatim
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
+ * \endverbatim
+ *
+ * @file
+ * @brief Parameters
+ */
+
+/* LIBC/STL */
+#include <algorithm>
+
+/* EXTERNAL */
+
+/* PACKAGE */
+#include "fixposition_driver_lib/params.hpp"
+
+namespace fixposition {
+/* ****************************************************************************************************************** */
+
+bool DriverParams::MessageEnabled(const std::string& message_name) const {
+    return std::find(messages_.cbegin(), messages_.cend(), message_name) != messages_.end();
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+bool StrToEpoch(const std::string& str, fpsdk::common::parser::fpa::FpaEpoch& epoch) {
+    // clang-format off
+    if      (str == "GNSS1")  { epoch = fpsdk::common::parser::fpa::FpaEpoch::GNSS1;       }
+    else if (str == "GNSS2")  { epoch = fpsdk::common::parser::fpa::FpaEpoch::GNSS2;       }
+    else if (str == "GNSS")   { epoch = fpsdk::common::parser::fpa::FpaEpoch::GNSS;        }
+    else if (str == "FUSION") { epoch = fpsdk::common::parser::fpa::FpaEpoch::FUSION;      }
+    else if (str.empty())     { epoch = fpsdk::common::parser::fpa::FpaEpoch::UNSPECIFIED; }
+    // clang-format on
+    else {
+        return false;
+    }
+    return true;
+}
+
+/* ****************************************************************************************************************** */
+}  // namespace fixposition
diff --git a/fixposition_driver_lib/src/parser.cpp b/fixposition_driver_lib/src/parser.cpp
deleted file mode 100644
index 73dfd38..0000000
--- a/fixposition_driver_lib/src/parser.cpp
+++ /dev/null
@@ -1,139 +0,0 @@
-/**
- *  @file
- *  @brief Parser functions
- *
- * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * \endverbatim
- *
- */
-
-/* PACKAGE */
-#include <fixposition_driver_lib/helper.hpp>
-#include <fixposition_driver_lib/messages/nov_type.hpp>
-
-namespace fixposition {
-
-static constexpr char kNmeaPreamble = '$';
-static constexpr int kLibParserMaxNmeaSize = 400;
-static constexpr int kLibParserMaxNovSize = 4096;
-
-int IsNmeaMessage(const char* buf, const int size) {
-    // Start of sentence
-    if (buf[0] != kNmeaPreamble) {
-        return 0;
-    }
-
-    // Find end of sentence, calculate checksum along the way
-    int len = 1;  // Length of sentence excl. "$"
-    char ck = 0;  // checksum
-    while (true) {
-        if (len > kLibParserMaxNmeaSize) {
-            return 0;
-        }
-        if (len >= size)  // len doesn't include '$'
-        {
-            return -1;
-        }
-        if ((buf[len] == '\r') || (buf[len] == '\n') || (buf[len] == '*')) {
-            break;
-        }
-        if (                                           // ((buf[len] & 0x80) != 0) || // 7-bit only
-            (buf[len] < 0x20) || (buf[len] > 0x7e) ||  // valid range
-            (buf[len] == '$') || (buf[len] == '\\') || (buf[len] == '!') || (buf[len] == '~'))  // reserved
-        {
-            return 0;
-        }
-        ck ^= buf[len];
-        len++;
-    }
-
-    // Not nough data for sentence end (star + checksum + \r\n)?
-    if (size < (len + 1 + 2 + 2)) {
-        return -1;
-    }
-
-    // Properly terminated sentence?
-    if ((buf[len] == '*') && (buf[len + 3] == '\r') && (buf[len + 4] == '\n')) {
-        char n1 = buf[len + 1];
-        char n2 = buf[len + 2];
-        char c1 = '0' + ((ck >> 4) & 0x0f);
-        char c2 = '0' + (ck & 0x0f);
-        if (c2 > '9') {
-            c2 += 'A' - '9' - 1;
-        }
-        // Checksum valid?
-        if ((n1 == c1) && (n2 == c2)) {
-            return len + 5;
-        }
-    }
-    return 0;
-}
-
-int IsNovMessage(const uint8_t* buf, const int size) {
-    if (buf[0] != SYNC_CHAR_1) {
-        return 0;
-    }
-
-    if (size < 3) {
-        return -1;
-    }
-
-    if ((buf[1] != SYNC_CHAR_2) || ((buf[2] != SYNC_CHAR_3_LONG) && (buf[2] != SYNC_CHAR_3_SHORT))) {
-        return 0;
-    }
-
-    // https://docs.novatel.com/OEM7/Content/Messages/Binary.htm
-    // https://docs.novatel.com/OEM7/Content/Messages/Description_of_Short_Headers.htm
-    // Offset  Type      Long header   Short header
-    // 0       uint8_t   0xaa         uint8_t   0xaa
-    // 1       uint8_t   0x44         uint8_t   0x44
-    // 2       uint8_t   0x12         uint8_t   0x13
-    // 3       uint8_t   header len   uint8_t   msg len
-    // 4       uint16_t  msg ID       uint16_t  msg ID
-    // 6       uint8_t   msg type     uint16_t  wno
-    // 7       uint8_t   port addr
-    // 8       uint16_t  msg len      int32_t   tow
-    // ...
-    // 3+hlen  payload             offs 13 payload
-
-    if (size < 12) {
-        return -1;
-    }
-
-    int len = 0;
-
-    // Long header
-    if (buf[2] == SYNC_CHAR_3_LONG) {
-        const uint8_t headerLen = buf[3];
-        const uint16_t msgLen = ((uint16_t)buf[9] << 8) | (uint16_t)buf[8];
-        len = headerLen + msgLen + sizeof(uint32_t);
-    }
-    // Short header
-    else {
-        const uint8_t headerLen = 12;
-        const uint8_t msgLen = buf[3];
-        len = headerLen + msgLen + sizeof(uint32_t);
-    }
-
-    if (len > kLibParserMaxNovSize) {
-        return 0;
-    }
-
-    if (size < len) {
-        return -1;
-    }
-
-    const uint32_t crc = (buf[len - 1] << 24) | (buf[len - 2] << 16) | (buf[len - 3] << 8) | (buf[len - 4]);
-    if (crc == nov_crc32(buf, len - sizeof(uint32_t))) {
-        return len;
-    } else {
-        return 0;
-    }
-}
-
-}  // namespace fixposition
diff --git a/fixposition_driver_msgs/CMakeLists.txt b/fixposition_driver_msgs/CMakeLists.txt
new file mode 100644
index 0000000..c3e5f0a
--- /dev/null
+++ b/fixposition_driver_msgs/CMakeLists.txt
@@ -0,0 +1,149 @@
+# GENERAL ==============================================================================================================
+
+cmake_minimum_required(VERSION 3.16)
+project(fixposition_driver_msgs VERSION 8.0.0 LANGUAGES CXX C)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror\
+    -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+    add_compile_definitions(NDEBUG)
+endif()
+
+
+# DEPENDENCIES =========================================================================================================
+
+# This gives us ROS_VERSION
+find_package(ros_environment REQUIRED)
+set(ROS_VERSION $ENV{ROS_VERSION})
+
+find_package(Eigen3 REQUIRED)
+find_package(fpsdk_common REQUIRED)
+find_package(fixposition_driver_lib REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+
+# BUILD, INSTALL =======================================================================================================
+
+# --- ROS 1 ------------------------------------------------------------------------------------------------------------
+if(${ROS_VERSION} EQUAL 1)
+
+    find_package(catkin REQUIRED COMPONENTS
+        #message_generation
+        std_msgs
+        geometry_msgs
+        nav_msgs
+        sensor_msgs
+    )
+
+    file(GLOB MSG_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}/msg msg/*.msg)
+    add_message_files(FILES ${MSG_FILES})
+
+    generate_messages(DEPENDENCIES
+        std_msgs geometry_msgs nav_msgs sensor_msgs
+    )
+
+    catkin_package(
+        INCLUDE_DIRS include
+        LIBRARIES ${PROJECT_NAME}
+        CATKIN_DEPENDS
+            #message_runtime
+            std_msgs geometry_msgs nav_msgs sensor_msgs
+    )
+
+    include_directories(
+        include
+        ${catkin_INCLUDE_DIRS}
+        ${fixposition_driver_lib_INCLUDE_DIRS}
+        ${fpsdk_common_INLCUDE_DIRS}
+        ${EIGEN3_INCLUDE_DIR}
+    )
+
+    add_library(${PROJECT_NAME} SHARED
+        src/data_to_ros.cpp
+    )
+
+    target_link_libraries(${PROJECT_NAME}
+        ${catkin_LIBRARIES}
+        ${fixposition_driver_lib_LIBRARIES}
+        ${fpsdk_common_LIBRARIES}
+    )
+
+    install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+    install(TARGETS ${PROJECT_NAME}
+        #EXPORT ${PROJECT_NAME}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+    )
+
+# --- ROS 2 ------------------------------------------------------------------------------------------------------------
+elseif(${ROS_VERSION} EQUAL 2)
+
+    # https://answers.ros.org/question/357633/
+
+    find_package(ament_cmake REQUIRED)
+    find_package(builtin_interfaces REQUIRED)
+    find_package(rosidl_default_generators REQUIRED)
+    find_package(rclcpp REQUIRED)
+
+    file(GLOB MSG_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg)
+    rosidl_generate_interfaces(${PROJECT_NAME} ADD_LINTER_TESTS
+        ${MSG_FILES}
+        DEPENDENCIES
+            builtin_interfaces
+            std_msgs geometry_msgs nav_msgs sensor_msgs
+    )
+
+    ament_export_dependencies(rosidl_default_runtime)
+
+    rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
+
+    add_library(${PROJECT_NAME}_lib SHARED
+        src/data_to_ros.cpp
+    )
+    target_include_directories(${PROJECT_NAME}_lib PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>
+        $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+        ${fixposition_driver_lib_INCLUDE_DIRS}
+        ${fpsdk_common_INLCUDE_DIRS}
+        ${EIGEN3_INCLUDE_DIR}
+    )
+
+    target_link_libraries(${PROJECT_NAME}_lib PUBLIC
+        ${cpp_typesupport_target}
+        ${fixposition_driver_lib_LIBRARIES}
+        ${fpsdk_common_LIBRARIES}
+        ${fpsdk_common_LIBRARIES}
+    )
+
+    install(DIRECTORY include/
+        DESTINATION include/${PROJECT_NAME}
+    )
+
+    install(
+        TARGETS ${PROJECT_NAME}_lib
+        EXPORT export_${PROJECT_NAME}_lib
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+    )
+
+    ament_export_include_directories(include)
+    ament_export_libraries(${PROJECT_NAME}_lib)
+    ament_export_targets(export_${PROJECT_NAME}_lib)
+
+    ament_package()
+
+endif()
diff --git a/fixposition_driver_msgs/README.md b/fixposition_driver_msgs/README.md
new file mode 100644
index 0000000..ef6d1f0
--- /dev/null
+++ b/fixposition_driver_msgs/README.md
@@ -0,0 +1,3 @@
+# Fixposition ROS Driver MEssages
+
+See the [main README.md](../README.md) for the documentation.
diff --git a/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp
new file mode 100644
index 0000000..65b4e39
--- /dev/null
+++ b/fixposition_driver_msgs/include/fixposition_driver_msgs/data_to_ros.hpp
@@ -0,0 +1,661 @@
+/**
+ * \verbatim
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
+ * \endverbatim
+ *
+ * @file
+ * @brief Helpers to convert data to ROS 1 and 2 msgs
+ */
+
+#ifndef __FIXPOSITION_DRIVER_MSGS_DATA_TO_ROS_HPP__
+#define __FIXPOSITION_DRIVER_MSGS_DATA_TO_ROS_HPP__
+
+/* LIBC/STL */
+
+/* EXTERNAL */
+#include <fpsdk_common/ext/eigen_core.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/nmea.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+
+/* PACKAGE */
+
+namespace fixposition {
+/* ****************************************************************************************************************** */
+
+// FpaFloat3 -> geometry_msgs::Vector3
+template <typename RosMsgVector3T>
+inline void FpaFloat3ToVector3(const fpsdk::common::parser::fpa::FpaFloat3 float3, RosMsgVector3T& vector3) {
+    if (float3.valid) {
+        vector3.x = float3.values[0];
+        vector3.y = float3.values[1];
+        vector3.z = float3.values[2];
+    }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+// FpaFloat6 (covariance) -> builtin_types::float64[9]
+template <typename RosMsgArray9T>
+inline void FpaFloat6ToArray9(const fpsdk::common::parser::fpa::FpaFloat6 float6, RosMsgArray9T& array9) {
+    if (float6.valid) {
+        // clang-format off
+        array9[0] = float6.values[0]; array9[1] = float6.values[3]; array9[2] = float6.values[5]; // 0 3 5   xx xy xz
+        array9[3] = float6.values[3]; array9[4] = float6.values[1]; array9[5] = float6.values[4]; // 3 1 4   xy yy yz
+        array9[6] = float6.values[5]; array9[7] = float6.values[4]; array9[8] = float6.values[2]; // 5 4 2   xz yz zz
+        // clang-format on
+    }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template <typename RosMsgT>
+inline int FpaGnssFixToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaGnssFix fix) {
+    // clang-format off
+    switch (fix) {
+        case fpsdk::common::parser::fpa::FpaGnssFix::UNSPECIFIED: return msg.consts.GNSS_FIX_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaGnssFix::UNKNOWN:     return msg.consts.GNSS_FIX_UNKNOWN;
+        case fpsdk::common::parser::fpa::FpaGnssFix::NOFIX:       return msg.consts.GNSS_FIX_NOFIX;
+        case fpsdk::common::parser::fpa::FpaGnssFix::DRONLY:      return msg.consts.GNSS_FIX_DRONLY;
+        case fpsdk::common::parser::fpa::FpaGnssFix::TIME:        return msg.consts.GNSS_FIX_TIME;
+        case fpsdk::common::parser::fpa::FpaGnssFix::S2D:         return msg.consts.GNSS_FIX_S2D;
+        case fpsdk::common::parser::fpa::FpaGnssFix::S3D:         return msg.consts.GNSS_FIX_S3D;
+        case fpsdk::common::parser::fpa::FpaGnssFix::S3D_DR:      return msg.consts.GNSS_FIX_S3D_DR;
+        case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FLOAT:   return msg.consts.GNSS_FIX_RTK_FLOAT;
+        case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FIXED:   return msg.consts.GNSS_FIX_RTK_FIXED;
+    }
+    // clang-format on
+    return msg.consts.GNSS_FIX_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaGnssFixToNavSatStatusStatus(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaGnssFix fix) {
+    // clang-format off
+    switch (fix) {
+        case fpsdk::common::parser::fpa::FpaGnssFix::UNSPECIFIED: /* FALLTHROUGH */
+        case fpsdk::common::parser::fpa::FpaGnssFix::UNKNOWN:     /* FALLTHROUGH */
+        case fpsdk::common::parser::fpa::FpaGnssFix::NOFIX:       /* FALLTHROUGH */
+        case fpsdk::common::parser::fpa::FpaGnssFix::DRONLY:      /* FALLTHROUGH */
+        case fpsdk::common::parser::fpa::FpaGnssFix::TIME:        return msg.STATUS_NO_FIX;
+        case fpsdk::common::parser::fpa::FpaGnssFix::S2D:         /* FALLTHROUGH */
+        case fpsdk::common::parser::fpa::FpaGnssFix::S3D:         /* FALLTHROUGH */
+        case fpsdk::common::parser::fpa::FpaGnssFix::S3D_DR:      return msg.STATUS_FIX;
+        case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FLOAT:   /* FALLTHROUGH */
+        case fpsdk::common::parser::fpa::FpaGnssFix::RTK_FIXED:   return msg.STATUS_GBAS_FIX;
+    }
+    // clang-format on
+    return msg.STATUS_NO_FIX;
+}
+
+template <typename RosMsgT>
+inline int FpaFusionStatusLegacyToMsg(const RosMsgT& msg,
+                                      const fpsdk::common::parser::fpa::FpaFusionStatusLegacy fusion_status) {
+    // clang-format off
+    switch (fusion_status) {
+        case fpsdk::common::parser::fpa::FpaFusionStatusLegacy::UNSPECIFIED: return msg.consts.FUSION_STATUS_LEGACY_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaFusionStatusLegacy::NONE:        return msg.consts.FUSION_STATUS_LEGACY_NONE;
+        case fpsdk::common::parser::fpa::FpaFusionStatusLegacy::VISION:      return msg.consts.FUSION_STATUS_LEGACY_VISION;
+        case fpsdk::common::parser::fpa::FpaFusionStatusLegacy::VIO:         return msg.consts.FUSION_STATUS_LEGACY_VIO;
+        case fpsdk::common::parser::fpa::FpaFusionStatusLegacy::IMU_GNSS:    return msg.consts.FUSION_STATUS_LEGACY_IMU_GNSS;
+        case fpsdk::common::parser::fpa::FpaFusionStatusLegacy::VIO_GNSS:    return msg.consts.FUSION_STATUS_LEGACY_VIO_GNSS;
+    }
+    // clang-format on
+    return msg.consts.FUSION_STATUS_LEGACY_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaImuStatusLegacyToMsg(const RosMsgT& msg,
+                                   const fpsdk::common::parser::fpa::FpaImuStatusLegacy imu_bias_status) {
+    // clang-format off
+    switch (imu_bias_status) {
+        case fpsdk::common::parser::fpa::FpaImuStatusLegacy::UNSPECIFIED:   return msg.consts.IMU_STATUS_LEGACY_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaImuStatusLegacy::NOT_CONVERGED: return msg.consts.IMU_STATUS_LEGACY_NOT_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaImuStatusLegacy::CONVERGED:     return msg.consts.IMU_STATUS_LEGACY_CONVERGED;
+    }
+    // clang-format on
+    return msg.consts.IMU_STATUS_LEGACY_UNSPECIFIED;
+    ;
+}
+
+template <typename RosMsgT>
+inline int FpaWsStatusLegacyToMsg(const RosMsgT& msg,
+                                  const fpsdk::common::parser::fpa::FpaWsStatusLegacy wheelspeed_status) {
+    // clang-format off
+    switch (wheelspeed_status) {
+        case fpsdk::common::parser::fpa::FpaWsStatusLegacy::UNSPECIFIED:           return msg.consts.WS_STATUS_LEGACY_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaWsStatusLegacy::NOT_ENABLED:           return msg.consts.WS_STATUS_LEGACY_NOT_ENABLED;
+        case fpsdk::common::parser::fpa::FpaWsStatusLegacy::NONE_CONVERGED:        return msg.consts.WS_STATUS_LEGACY_NONE_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaWsStatusLegacy::ONE_OR_MORE_CONVERGED: return msg.consts.WS_STATUS_LEGACY_ONE_OR_MORE_CONVERGED;
+    }
+    // clang-format on
+    return msg.consts.WS_STATUS_LEGACY_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaInitStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaInitStatus init_status) {
+    // clang-format off
+    switch (init_status) {
+        case fpsdk::common::parser::fpa::FpaInitStatus::UNSPECIFIED: return msg.consts.INIT_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaInitStatus::NOT_INIT:    return msg.consts.INIT_STATUS_NOT_INIT;
+        case fpsdk::common::parser::fpa::FpaInitStatus::LOCAL_INIT:  return msg.consts.INIT_STATUS_LOCAL_INIT;
+        case fpsdk::common::parser::fpa::FpaInitStatus::GLOBAL_INIT: return msg.consts.INIT_STATUS_GLOBAL_INIT;
+    }
+    // clang-format on
+    return msg.consts.INIT_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaMeasStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaMeasStatus status) {
+    // clang-format off
+    switch (status) {
+        case fpsdk::common::parser::fpa::FpaMeasStatus::UNSPECIFIED: return msg.consts.MEAS_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaMeasStatus::NOT_USED:    return msg.consts.MEAS_STATUS_NOT_USED;
+        case fpsdk::common::parser::fpa::FpaMeasStatus::USED:        return msg.consts.MEAS_STATUS_USED;
+        case fpsdk::common::parser::fpa::FpaMeasStatus::DEGRADED:    return msg.consts.MEAS_STATUS_DEGRADED;
+    }
+    // clang-format on
+    return msg.consts.MEAS_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaImuStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaImuStatus imu_status) {
+    // clang-format off
+    switch (imu_status) {
+        case fpsdk::common::parser::fpa::FpaImuStatus::UNSPECIFIED:     return msg.consts.IMU_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaImuStatus::NOT_CONVERGED:   return msg.consts.IMU_STATUS_NOT_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaImuStatus::WARMSTARTED:     return msg.consts.IMU_STATUS_WARMSTARTED;
+        case fpsdk::common::parser::fpa::FpaImuStatus::ROUGH_CONVERGED: return msg.consts.IMU_STATUS_ROUGH_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaImuStatus::FINE_CONVERGED:  return msg.consts.IMU_STATUS_FINE_CONVERGED;
+    }
+    // clang-format on
+    return msg.consts.IMU_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaImuNoiseToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaImuNoise imu_noise) {
+    // clang-format off
+    switch (imu_noise) {
+        case fpsdk::common::parser::fpa::FpaImuNoise::UNSPECIFIED:  return msg.consts.IMU_NOISE_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaImuNoise::LOW_NOISE:    return msg.consts.IMU_NOISE_LOW_NOISE;
+        case fpsdk::common::parser::fpa::FpaImuNoise::MEDIUM_NOISE: return msg.consts.IMU_NOISE_MEDIUM_NOISE;
+        case fpsdk::common::parser::fpa::FpaImuNoise::HIGH_NOISE:   return msg.consts.IMU_NOISE_HIGH_NOISE;
+        case fpsdk::common::parser::fpa::FpaImuNoise::RESERVED4:    return msg.consts.IMU_NOISE_RESERVED4;
+        case fpsdk::common::parser::fpa::FpaImuNoise::RESERVED5:    return msg.consts.IMU_NOISE_RESERVED5;
+        case fpsdk::common::parser::fpa::FpaImuNoise::RESERVED6:    return msg.consts.IMU_NOISE_RESERVED6;
+        case fpsdk::common::parser::fpa::FpaImuNoise::RESERVED7:    return msg.consts.IMU_NOISE_RESERVED7;
+    }
+    // clang-format on
+    return msg.consts.IMU_NOISE_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaImuConvToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaImuConv imu_conv) {
+    // clang-format off
+    switch (imu_conv) {
+        case fpsdk::common::parser::fpa::FpaImuConv::UNSPECIFIED:      return msg.consts.IMU_CONV_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaImuConv::RESERVED0:        return msg.consts.IMU_CONV_RESERVED0;
+        case fpsdk::common::parser::fpa::FpaImuConv::WAIT_IMU_MEAS:    return msg.consts.IMU_CONV_WAIT_IMU_MEAS;
+        case fpsdk::common::parser::fpa::FpaImuConv::WAIT_GLOBAL_MEAS: return msg.consts.IMU_CONV_WAIT_GLOBAL_MEAS;
+        case fpsdk::common::parser::fpa::FpaImuConv::WAIT_MOTION:      return msg.consts.IMU_CONV_WAIT_MOTION;
+        case fpsdk::common::parser::fpa::FpaImuConv::CONVERGING:       return msg.consts.IMU_CONV_CONVERGING;
+        case fpsdk::common::parser::fpa::FpaImuConv::RESERVED5:        return msg.consts.IMU_CONV_RESERVED5;
+        case fpsdk::common::parser::fpa::FpaImuConv::RESERVED6:        return msg.consts.IMU_CONV_RESERVED6;
+        case fpsdk::common::parser::fpa::FpaImuConv::IDLE:             return msg.consts.IMU_CONV_IDLE;
+    }
+    // clang-format on
+    return msg.consts.IMU_CONV_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaGnssStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaGnssStatus status) {
+    // clang-format off
+    switch (status) {
+        case fpsdk::common::parser::fpa::FpaGnssStatus::UNSPECIFIED: return msg.consts.GNSS_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::NO_FIX:      return msg.consts.GNSS_STATUS_NO_FIX;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::SPP:         return msg.consts.GNSS_STATUS_SPP;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RTK_MB:      return msg.consts.GNSS_STATUS_RTK_MB;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED3:   return msg.consts.GNSS_STATUS_RESERVED3;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED4:   return msg.consts.GNSS_STATUS_RESERVED4;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RTK_FLOAT:   return msg.consts.GNSS_STATUS_RTK_FLOAT;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED6:   return msg.consts.GNSS_STATUS_RESERVED6;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED7:   return msg.consts.GNSS_STATUS_RESERVED7;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RTK_FIXED:   return msg.consts.GNSS_STATUS_RTK_FIXED;
+        case fpsdk::common::parser::fpa::FpaGnssStatus::RESERVED9:   return msg.consts.GNSS_STATUS_RESERVED9;
+    }
+    // clang-format on
+    return msg.consts.GNSS_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaBaselineStatusToMsg(const RosMsgT& msg,
+                                  const fpsdk::common::parser::fpa::FpaBaselineStatus baseline_status) {
+    // clang-format off
+    switch (baseline_status) {
+        case fpsdk::common::parser::fpa::FpaBaselineStatus::UNSPECIFIED:    return msg.consts.BASELINE_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaBaselineStatus::WAITING_FUSION: return msg.consts.BASELINE_STATUS_WAITING_FUSION;
+        case fpsdk::common::parser::fpa::FpaBaselineStatus::NO_FIX:         return msg.consts.BASELINE_STATUS_NO_FIX;
+        case fpsdk::common::parser::fpa::FpaBaselineStatus::FAILING:        return msg.consts.BASELINE_STATUS_FAILING;
+        case fpsdk::common::parser::fpa::FpaBaselineStatus::PASSING:        return msg.consts.BASELINE_STATUS_PASSING;
+    }
+    // clang-format on
+    return msg.consts.BASELINE_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaCorrStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaCorrStatus corr_status) {
+    // clang-format off
+    switch (corr_status) {
+        case fpsdk::common::parser::fpa::FpaCorrStatus::UNSPECIFIED:    return msg.consts.CORR_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::WAITING_FUSION: return msg.consts.CORR_STATUS_WAITING_FUSION;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::NO_GNSS:        return msg.consts.CORR_STATUS_NO_GNSS;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::NO_CORR:        return msg.consts.CORR_STATUS_NO_CORR;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::LIMITED_CORR:   return msg.consts.CORR_STATUS_LIMITED_CORR;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::OLD_CORR:       return msg.consts.CORR_STATUS_OLD_CORR;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::GOOD_CORR:      return msg.consts.CORR_STATUS_GOOD_CORR;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::RESERVED6:      return msg.consts.CORR_STATUS_RESERVED6;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::RESERVED7:      return msg.consts.CORR_STATUS_RESERVED7;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::RESERVED8:      return msg.consts.CORR_STATUS_RESERVED8;
+        case fpsdk::common::parser::fpa::FpaCorrStatus::RESERVED9:      return msg.consts.CORR_STATUS_RESERVED9;
+    }
+    // clang-format on
+    return msg.consts.CORR_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaCamStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaCamStatus cam_status) {
+    // clang-format off
+    switch (cam_status) {
+        case fpsdk::common::parser::fpa::FpaCamStatus::UNSPECIFIED: return msg.consts.CAM_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaCamStatus::CAM_UNAVL:   return msg.consts.CAM_STATUS_CAM_UNAVL;
+        case fpsdk::common::parser::fpa::FpaCamStatus::BAD_FEAT:    return msg.consts.CAM_STATUS_BAD_FEAT;
+        case fpsdk::common::parser::fpa::FpaCamStatus::RESERVED2:   return msg.consts.CAM_STATUS_RESERVED2;
+        case fpsdk::common::parser::fpa::FpaCamStatus::RESERVED3:   return msg.consts.CAM_STATUS_RESERVED3;
+        case fpsdk::common::parser::fpa::FpaCamStatus::RESERVED4:   return msg.consts.CAM_STATUS_RESERVED4;
+        case fpsdk::common::parser::fpa::FpaCamStatus::GOOD:        return msg.consts.CAM_STATUS_GOOD;
+    }
+    // clang-format on
+    return msg.consts.CAM_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaWsStatusToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaWsStatus ws_status) {
+    // clang-format off
+    switch (ws_status) {
+        case fpsdk::common::parser::fpa::FpaWsStatus::UNSPECIFIED:    return msg.consts.WS_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaWsStatus::NOT_ENABLED:    return msg.consts.WS_STATUS_NOT_ENABLED;
+        case fpsdk::common::parser::fpa::FpaWsStatus::MISS_MEAS:      return msg.consts.WS_STATUS_MISS_MEAS;
+        case fpsdk::common::parser::fpa::FpaWsStatus::NONE_CONVERGED: return msg.consts.WS_STATUS_NONE_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaWsStatus::ONE_CONVERGED:  return msg.consts.WS_STATUS_ONE_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaWsStatus::ALL_CONVERGED:  return msg.consts.WS_STATUS_ALL_CONVERGED;
+    }
+    // clang-format on
+    return msg.consts.WS_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaWsConvToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaWsConv ws_conv) {
+    // clang-format off
+    switch (ws_conv) {
+        case fpsdk::common::parser::fpa::FpaWsConv::UNSPECIFIED:      return msg.consts.WS_CONV_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaWsConv::WAIT_FUSION:      return msg.consts.WS_CONV_WAIT_FUSION;
+        case fpsdk::common::parser::fpa::FpaWsConv::WAIT_WS_MEAS:     return msg.consts.WS_CONV_WAIT_WS_MEAS;
+        case fpsdk::common::parser::fpa::FpaWsConv::WAIT_GLOBAL_MEAS: return msg.consts.WS_CONV_WAIT_GLOBAL_MEAS;
+        case fpsdk::common::parser::fpa::FpaWsConv::WAIT_MOTION:      return msg.consts.WS_CONV_WAIT_MOTION;
+        case fpsdk::common::parser::fpa::FpaWsConv::WAIT_IMU_BIAS:    return msg.consts.WS_CONV_WAIT_IMU_BIAS;
+        case fpsdk::common::parser::fpa::FpaWsConv::CONVERGING:       return msg.consts.WS_CONV_CONVERGING;
+        case fpsdk::common::parser::fpa::FpaWsConv::IDLE:             return msg.consts.WS_CONV_IDLE;
+    }
+    // clang-format on
+    return msg.consts.WS_CONV_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaMarkersStatusToMsg(const RosMsgT& msg,
+                                 const fpsdk::common::parser::fpa::FpaMarkersStatus markers_status) {
+    // clang-format off
+    switch (markers_status) {
+        case fpsdk::common::parser::fpa::FpaMarkersStatus::UNSPECIFIED:    return msg.consts.MARKERS_STATUS_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaMarkersStatus::NOT_ENABLED:    return msg.consts.MARKERS_STATUS_NOT_ENABLED;
+        case fpsdk::common::parser::fpa::FpaMarkersStatus::NONE_CONVERGED: return msg.consts.MARKERS_STATUS_NONE_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaMarkersStatus::ONE_CONVERGED:  return msg.consts.MARKERS_STATUS_ONE_CONVERGED;
+        case fpsdk::common::parser::fpa::FpaMarkersStatus::ALL_CONVERGED:  return msg.consts.MARKERS_STATUS_ALL_CONVERGED;
+    }
+    // clang-format on
+    return msg.consts.MARKERS_STATUS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaMarkersConvToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaMarkersConv markers_conv) {
+    // clang-format off
+    switch (markers_conv) {
+        case fpsdk::common::parser::fpa::FpaMarkersConv::UNSPECIFIED:      return msg.consts.MARKERS_CONV_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaMarkersConv::WAIT_FUSION:      return msg.consts.MARKERS_CONV_WAIT_FUSION;
+        case fpsdk::common::parser::fpa::FpaMarkersConv::WAIT_MARKER_MEAS: return msg.consts.MARKERS_CONV_WAIT_MARKER_MEAS;
+        case fpsdk::common::parser::fpa::FpaMarkersConv::WAIT_GLOBAL_MEAS: return msg.consts.MARKERS_CONV_WAIT_GLOBAL_MEAS;
+        case fpsdk::common::parser::fpa::FpaMarkersConv::CONVERGING:       return msg.consts.MARKERS_CONV_CONVERGING;
+        case fpsdk::common::parser::fpa::FpaMarkersConv::IDLE:             return msg.consts.MARKERS_CONV_IDLE;
+    }
+    // clang-format on
+    return msg.consts.MARKERS_CONV_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaAntStateToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaAntState ant_state) {
+    // clang-format off
+    switch (ant_state) {
+        case fpsdk::common::parser::fpa::FpaAntState::UNSPECIFIED: return msg.consts.ANT_STATE_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaAntState::OK:          return msg.consts.ANT_STATE_OPEN;
+        case fpsdk::common::parser::fpa::FpaAntState::OPEN:        return msg.consts.ANT_STATE_OK;
+        case fpsdk::common::parser::fpa::FpaAntState::SHORT:       return msg.consts.ANT_STATE_SHORT;
+    }
+    // clang-format on
+    return msg.consts.ANT_STATE_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaAntPowerToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaAntPower ant_power) {
+    // clang-format off
+    switch (ant_power) {
+        case fpsdk::common::parser::fpa::FpaAntPower::UNSPECIFIED: return msg.consts.ANT_POWER_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaAntPower::ON:          return msg.consts.ANT_POWER_OFF;
+        case fpsdk::common::parser::fpa::FpaAntPower::OFF:         return msg.consts.ANT_POWER_ON;
+    }
+    // clang-format on
+    return msg.consts.ANT_POWER_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaEpochToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaEpoch epoch) {
+    // clang-format off
+    switch (epoch)
+    {
+        case fpsdk::common::parser::fpa::FpaEpoch::UNSPECIFIED: return msg.consts.EPOCH_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaEpoch::GNSS1:       return msg.consts.EPOCH_GNSS1;
+        case fpsdk::common::parser::fpa::FpaEpoch::GNSS2:       return msg.consts.EPOCH_GNSS2;
+        case fpsdk::common::parser::fpa::FpaEpoch::GNSS:        return msg.consts.EPOCH_GNSS;
+        case fpsdk::common::parser::fpa::FpaEpoch::FUSION:      return msg.consts.EPOCH_FUSION;
+    }
+    // clang-format on
+    return msg.consts.EPOCH_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaTextLevelToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaTextLevel level) {
+    // clang-format off
+    switch (level)
+    {
+        case fpsdk::common::parser::fpa::FpaTextLevel::UNSPECIFIED: return msg.consts.TEXT_LEVEL_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaTextLevel::ERROR:       return msg.consts.TEXT_LEVEL_ERROR;
+        case fpsdk::common::parser::fpa::FpaTextLevel::WARNING:     return msg.consts.TEXT_LEVEL_WARNING;
+        case fpsdk::common::parser::fpa::FpaTextLevel::INFO:        return msg.consts.TEXT_LEVEL_INFO;
+        case fpsdk::common::parser::fpa::FpaTextLevel::DEBUG:       return msg.consts.TEXT_LEVEL_DEBUG;
+    }
+    // clang-format on
+    return msg.consts.TEXT_LEVEL_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaTimebaseToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaTimebase timebase) {
+    // clang-format off
+    switch (timebase)
+    {
+        case fpsdk::common::parser::fpa::FpaTimebase::UNSPECIFIED: return msg.consts.TIMEBASE_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaTimebase::NONE:        return msg.consts.TIMEBASE_NONE;
+        case fpsdk::common::parser::fpa::FpaTimebase::GNSS:        return msg.consts.TIMEBASE_GNSS;
+        case fpsdk::common::parser::fpa::FpaTimebase::UTC:         return msg.consts.TIMEBASE_UTC;
+    }
+    // clang-format on
+    return msg.consts.TIMEBASE_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int FpaTimerefToMsg(const RosMsgT& msg, const fpsdk::common::parser::fpa::FpaTimeref timeref) {
+    // clang-format off
+    switch (timeref)
+    {
+        case fpsdk::common::parser::fpa::FpaTimeref::UNSPECIFIED: return msg.consts.TIMEREF_UNSPECIFIED;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_NONE:    return msg.consts.TIMEREF_UTC_NONE;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_CRL:     return msg.consts.TIMEREF_UTC_CRL;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_NIST:    return msg.consts.TIMEREF_UTC_NIST;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_USNO:    return msg.consts.TIMEREF_UTC_USNO;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_BIPM:    return msg.consts.TIMEREF_UTC_BIPM;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_EU:      return msg.consts.TIMEREF_UTC_EU;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_SU:      return msg.consts.TIMEREF_UTC_SU;
+        case fpsdk::common::parser::fpa::FpaTimeref::UTC_NTSC:    return msg.consts.TIMEREF_UTC_NTSC;
+        case fpsdk::common::parser::fpa::FpaTimeref::GNSS_GPS:    return msg.consts.TIMEREF_GNSS_GPS;
+        case fpsdk::common::parser::fpa::FpaTimeref::GNSS_GAL:    return msg.consts.TIMEREF_GNSS_GAL;
+        case fpsdk::common::parser::fpa::FpaTimeref::GNSS_BDS:    return msg.consts.TIMEREF_GNSS_BDS;
+        case fpsdk::common::parser::fpa::FpaTimeref::GNSS_GLO:    return msg.consts.TIMEREF_GNSS_GLO;
+        case fpsdk::common::parser::fpa::FpaTimeref::GNSS_NVC:    return msg.consts.TIMEREF_GNSS_NVC;
+        case fpsdk::common::parser::fpa::FpaTimeref::OTHER:       return msg.consts.TIMEREF_OTHER;
+    }
+    // clang-format on
+    return msg.consts.TIMEREF_UNSPECIFIED;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template <typename RosMsgT>
+inline int NovbPosOrVelTypeToNavSatStatusStatus(const RosMsgT& msg,
+                                                const fpsdk::common::parser::novb::NovbPosOrVelType type) {
+    switch (type) {
+            // clang-format off
+        case fpsdk::common::parser::novb::NovbPosOrVelType::NARROW_INT:   return msg.STATUS_GBAS_FIX; break;
+        case fpsdk::common::parser::novb::NovbPosOrVelType::NARROW_FLOAT:
+        case fpsdk::common::parser::novb::NovbPosOrVelType::SINGLE:       return msg.STATUS_FIX;      break;
+        default:                                                          return msg.STATUS_NO_FIX;   break;
+            // clang-format on
+    }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template <typename RosMsgT>
+inline int NmeaTalkerIdToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaTalkerId talker) {
+    // clang-format off
+    switch (talker)
+    {
+        case fpsdk::common::parser::nmea::NmeaTalkerId::UNSPECIFIED: return msg.consts.TALKER_ID_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::PROPRIETARY: return msg.consts.TALKER_ID_PROPRIETARY;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::GPS_SBAS:    return msg.consts.TALKER_ID_GPS_SBAS;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::GLO:         return msg.consts.TALKER_ID_GLO;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::GAL:         return msg.consts.TALKER_ID_GAL;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::BDS:         return msg.consts.TALKER_ID_BDS;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::NAVIC:       return msg.consts.TALKER_ID_NAVIC;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::QZSS:        return msg.consts.TALKER_ID_QZSS;
+        case fpsdk::common::parser::nmea::NmeaTalkerId::GNSS:        return msg.consts.TALKER_ID_GNSS;
+    }
+    // clang-format on
+    return msg.consts.TALKER_ID_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaQualityGgaToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaQualityGga quality) {
+    // clang-format off
+    switch (quality)
+    {
+        case fpsdk::common::parser::nmea::NmeaQualityGga::UNSPECIFIED: return msg.consts.QUALITY_GGA_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::NOFIX:       return msg.consts.QUALITY_GGA_NOFIX;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::SPP:         return msg.consts.QUALITY_GGA_SPP;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::DGNSS:       return msg.consts.QUALITY_GGA_DGNSS;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::PPS:         return msg.consts.QUALITY_GGA_PPS;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::RTK_FIXED:   return msg.consts.QUALITY_GGA_RTK_FIXED;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::RTK_FLOAT:   return msg.consts.QUALITY_GGA_RTK_FLOAT;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::ESTIMATED:   return msg.consts.QUALITY_GGA_ESTIMATED;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::MANUAL:      return msg.consts.QUALITY_GGA_MANUAL;
+        case fpsdk::common::parser::nmea::NmeaQualityGga::SIM:         return msg.consts.QUALITY_GGA_SIM;
+    }
+    // clang-format on
+    return msg.consts.QUALITY_GGA_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaStatusGllRmcToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaStatusGllRmc status) {
+    // clang-format off
+    switch (status)
+    {
+        case fpsdk::common::parser::nmea::NmeaStatusGllRmc::UNSPECIFIED: return msg.consts.STATUS_GLL_RMC_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaStatusGllRmc::INVALID:     return msg.consts.STATUS_GLL_RMC_INVALID;
+        case fpsdk::common::parser::nmea::NmeaStatusGllRmc::VALID:       return msg.consts.STATUS_GLL_RMC_VALID;
+    }
+    // clang-format on
+    return msg.consts.STATUS_GLL_RMC_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaModeGllVtgToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaModeGllVtg mode) {
+    // clang-format off
+    switch (mode)
+    {
+        case fpsdk::common::parser::nmea::NmeaModeGllVtg::UNSPECIFIED: return msg.consts.MODE_GLL_VTG_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaModeGllVtg::INVALID:     return msg.consts.MODE_GLL_VTG_INVALID;
+        case fpsdk::common::parser::nmea::NmeaModeGllVtg::AUTONOMOUS:  return msg.consts.MODE_GLL_VTG_AUTONOMOUS;
+        case fpsdk::common::parser::nmea::NmeaModeGllVtg::DGNSS:       return msg.consts.MODE_GLL_VTG_DGNSS;
+        case fpsdk::common::parser::nmea::NmeaModeGllVtg::ESTIMATED:   return msg.consts.MODE_GLL_VTG_ESTIMATED;
+        case fpsdk::common::parser::nmea::NmeaModeGllVtg::MANUAL:      return msg.consts.MODE_GLL_VTG_MANUAL;
+        case fpsdk::common::parser::nmea::NmeaModeGllVtg::SIM:         return msg.consts.MODE_GLL_VTG_SIM;
+    }
+    // clang-format on
+    return msg.consts.MODE_GLL_VTG_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaModeRmcGnsToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaModeRmcGns mode) {
+    // clang-format off
+    switch (mode)
+    {
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::UNSPECIFIED: return msg.consts.MODE_RMC_GNS_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::INVALID:     return msg.consts.MODE_RMC_GNS_INVALID;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::AUTONOMOUS:  return msg.consts.MODE_RMC_GNS_AUTONOMOUS;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::DGNSS:       return msg.consts.MODE_RMC_GNS_DGNSS;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::ESTIMATED:   return msg.consts.MODE_RMC_GNS_ESTIMATED;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::RTK_FIXED:   return msg.consts.MODE_RMC_GNS_RTK_FIXED;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::RTK_FLOAT:   return msg.consts.MODE_RMC_GNS_RTK_FLOAT;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::PRECISE:     return msg.consts.MODE_RMC_GNS_PRECISE;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::MANUAL:      return msg.consts.MODE_RMC_GNS_MANUAL;
+        case fpsdk::common::parser::nmea::NmeaModeRmcGns::SIM:         return msg.consts.MODE_RMC_GNS_SIM;
+    }
+    // clang-format on
+    return msg.consts.MODE_RMC_GNS_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaNavStatusRmcToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaNavStatusRmc nav_status) {
+    // clang-format off
+    switch (nav_status)
+    {
+        case fpsdk::common::parser::nmea::NmeaNavStatusRmc::UNSPECIFIED: return msg.consts.NAV_STATUS_RMC_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaNavStatusRmc::NA:          return msg.consts.NAV_STATUS_RMC_NA;
+        case fpsdk::common::parser::nmea::NmeaNavStatusRmc::SAFE:        return msg.consts.NAV_STATUS_RMC_SAFE;
+        case fpsdk::common::parser::nmea::NmeaNavStatusRmc::CAUTION:     return msg.consts.NAV_STATUS_RMC_CAUTION;
+        case fpsdk::common::parser::nmea::NmeaNavStatusRmc::UNSAFE:      return msg.consts.NAV_STATUS_RMC_UNSAFE;
+    }
+    // clang-format on
+    return msg.consts.NAV_STATUS_RMC_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaOpModeGsaToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaOpModeGsa op_mode) {
+    // clang-format off
+    switch (op_mode)
+    {
+        case fpsdk::common::parser::nmea::NmeaOpModeGsa::UNSPECIFIED: return msg.consts.OP_MODE_GSA_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaOpModeGsa::MANUAL:      return msg.consts.OP_MODE_GSA_MANUAL;
+        case fpsdk::common::parser::nmea::NmeaOpModeGsa::AUTO:        return msg.consts.OP_MODE_GSA_AUTO;
+    }
+    // clang-format on
+    return msg.consts.OP_MODE_GSA_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaNavModeGsaToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaNavModeGsa nav_mode) {
+    // clang-format off
+    switch (nav_mode)
+    {
+        case fpsdk::common::parser::nmea::NmeaNavModeGsa::UNSPECIFIED: return msg.consts.NAV_MODE_GSA_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaNavModeGsa::NOFIX:       return msg.consts.NAV_MODE_GSA_NOFIX;
+        case fpsdk::common::parser::nmea::NmeaNavModeGsa::FIX2D:       return msg.consts.NAV_MODE_GSA_FIX2D;
+        case fpsdk::common::parser::nmea::NmeaNavModeGsa::FIX3D:       return msg.consts.NAV_MODE_GSA_FIX3D;
+    }
+    // clang-format on
+    return msg.consts.NAV_MODE_GSA_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaSystemIdToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaSystemId system) {
+    // clang-format off
+    switch (system)
+    {
+        case fpsdk::common::parser::nmea::NmeaSystemId::UNSPECIFIED: return msg.consts.SYSTEM_ID_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaSystemId::GPS_SBAS:    return msg.consts.SYSTEM_ID_GPS_SBAS;
+        case fpsdk::common::parser::nmea::NmeaSystemId::GLO:         return msg.consts.SYSTEM_ID_GLO;
+        case fpsdk::common::parser::nmea::NmeaSystemId::GAL:         return msg.consts.SYSTEM_ID_GAL;
+        case fpsdk::common::parser::nmea::NmeaSystemId::BDS:         return msg.consts.SYSTEM_ID_BDS;
+        case fpsdk::common::parser::nmea::NmeaSystemId::QZSS:        return msg.consts.SYSTEM_ID_QZSS;
+        case fpsdk::common::parser::nmea::NmeaSystemId::NAVIC:       return msg.consts.SYSTEM_ID_NAVIC;
+    }
+    // clang-format on
+    return msg.consts.SYSTEM_ID_UNSPECIFIED;
+}
+
+template <typename RosMsgT>
+inline int NmeaSignalIdToMsg(const RosMsgT& msg, const fpsdk::common::parser::nmea::NmeaSignalId signal) {
+    // clang-format off
+    switch (signal)
+    {
+        case fpsdk::common::parser::nmea::NmeaSignalId::UNSPECIFIED: return msg.consts.SIGNAL_ID_UNSPECIFIED;
+        case fpsdk::common::parser::nmea::NmeaSignalId::NONE:        return msg.consts.SIGNAL_ID_NONE;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GPS_L1CA:    return msg.consts.SIGNAL_ID_GPS_L1CA;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GPS_L2CL:    return msg.consts.SIGNAL_ID_GPS_L2CL;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GPS_L2CM:    return msg.consts.SIGNAL_ID_GPS_L2CM;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GPS_L5I:     return msg.consts.SIGNAL_ID_GPS_L5I;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GPS_L5Q:     return msg.consts.SIGNAL_ID_GPS_L5Q;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E1:      return msg.consts.SIGNAL_ID_GAL_E1;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E5A:     return msg.consts.SIGNAL_ID_GAL_E5A;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GAL_E5B:     return msg.consts.SIGNAL_ID_GAL_E5B;
+        case fpsdk::common::parser::nmea::NmeaSignalId::BDS_B1ID:    return msg.consts.SIGNAL_ID_BDS_B1ID;
+        case fpsdk::common::parser::nmea::NmeaSignalId::BDS_B2ID:    return msg.consts.SIGNAL_ID_BDS_B2ID;
+        case fpsdk::common::parser::nmea::NmeaSignalId::BDS_B1C:     return msg.consts.SIGNAL_ID_BDS_B1C;
+        case fpsdk::common::parser::nmea::NmeaSignalId::BDS_B2A:     return msg.consts.SIGNAL_ID_BDS_B2A;
+        case fpsdk::common::parser::nmea::NmeaSignalId::QZSS_L1CA:   return msg.consts.SIGNAL_ID_QZSS_L1CA;
+        case fpsdk::common::parser::nmea::NmeaSignalId::QZSS_L1S:    return msg.consts.SIGNAL_ID_QZSS_L1S;
+        case fpsdk::common::parser::nmea::NmeaSignalId::QZSS_L2CM:   return msg.consts.SIGNAL_ID_QZSS_L2CM;
+        case fpsdk::common::parser::nmea::NmeaSignalId::QZSS_L2CL:   return msg.consts.SIGNAL_ID_QZSS_L2CL;
+        case fpsdk::common::parser::nmea::NmeaSignalId::QZSS_L5I:    return msg.consts.SIGNAL_ID_QZSS_L5I;
+        case fpsdk::common::parser::nmea::NmeaSignalId::QZSS_L5Q:    return msg.consts.SIGNAL_ID_QZSS_L5Q;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GLO_L1OF:    return msg.consts.SIGNAL_ID_GLO_L1OF;
+        case fpsdk::common::parser::nmea::NmeaSignalId::GLO_L2OF:    return msg.consts.SIGNAL_ID_GLO_L2OF;
+        case fpsdk::common::parser::nmea::NmeaSignalId::NAVIC_L5A:   return msg.consts.SIGNAL_ID_NAVIC_L5A;
+    }
+    // clang-format on
+    return msg.consts.SIGNAL_ID_UNSPECIFIED;
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+template <typename RosMsgT>
+inline int ParserProtocolToMsg(const RosMsgT& msg, const fpsdk::common::parser::Protocol protocol) {
+    // clang-format off
+    switch (protocol)
+    {
+        case fpsdk::common::parser::Protocol::FP_A:   return msg.PROTOCOL_FP_A;
+        case fpsdk::common::parser::Protocol::FP_B:   return msg.PROTOCOL_FP_B;
+        case fpsdk::common::parser::Protocol::NMEA:   return msg.PROTOCOL_NMEA;
+        case fpsdk::common::parser::Protocol::UBX:    return msg.PROTOCOL_UBX;
+        case fpsdk::common::parser::Protocol::RTCM3:  return msg.PROTOCOL_RTCM3;
+        case fpsdk::common::parser::Protocol::UNI_B:  return msg.PROTOCOL_UNI_B;
+        case fpsdk::common::parser::Protocol::NOV_B:  return msg.PROTOCOL_NOV_B;
+        case fpsdk::common::parser::Protocol::SPARTN: return msg.PROTOCOL_SPARTN;
+        case fpsdk::common::parser::Protocol::OTHER:  return msg.PROTOCOL_OTHER;
+    }
+    // clang-format on
+    return msg.PROTOCOL_UNSPECIFIED;
+}
+
+/* ****************************************************************************************************************** */
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_MSGS_DATA_TO_ROS_HPP__
diff --git a/fixposition_driver_msgs/msg/CovWarn.msg b/fixposition_driver_msgs/msg/CovWarn.msg
new file mode 100644
index 0000000..71456e0
--- /dev/null
+++ b/fixposition_driver_msgs/msg/CovWarn.msg
@@ -0,0 +1,8 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# Covariance warning message
+
+std_msgs/Header header
+geometry_msgs/Vector3 jump         # Position change
+geometry_msgs/Vector3 covariance   # Covariance
diff --git a/fixposition_driver_msgs/msg/FpaConsts.msg b/fixposition_driver_msgs/msg/FpaConsts.msg
new file mode 100644
index 0000000..0093102
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaConsts.msg
@@ -0,0 +1,198 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A enum constants
+
+# FpaFusionStatusLegacy
+int8   FUSION_STATUS_LEGACY_UNSPECIFIED            =  -1  # Unspecified
+int8   FUSION_STATUS_LEGACY_NONE                   =   0  # Not started
+int8   FUSION_STATUS_LEGACY_VISION                 =   1  # Vision only
+int8   FUSION_STATUS_LEGACY_VIO                    =   2  # Visual-inertial fusion
+int8   FUSION_STATUS_LEGACY_IMU_GNSS               =   3  # Inertial-GNSS fusion
+int8   FUSION_STATUS_LEGACY_VIO_GNSS               =   4  # Visual-inertial-GNSS fusion
+
+# FpaImuStatusLegacy
+int8   IMU_STATUS_LEGACY_UNSPECIFIED               =  -1  # Unspecified
+int8   IMU_STATUS_LEGACY_NOT_CONVERGED             =   0  # Not converged
+int8   IMU_STATUS_LEGACY_CONVERGED                 =   1  # Converged
+
+# FpaWsStatusLegacy
+int8   WS_STATUS_LEGACY_UNSPECIFIED                =  -2  # Unspecified
+int8   WS_STATUS_LEGACY_NOT_ENABLED                =  -1  # No wheelspeed enabled
+int8   WS_STATUS_LEGACY_NONE_CONVERGED             =   0  # None converged
+int8   WS_STATUS_LEGACY_ONE_OR_MORE_CONVERGED      =   1  # At least one converged
+
+# FpaInitStatus
+int8   INIT_STATUS_UNSPECIFIED                     =  -1  # Unspecified
+int8   INIT_STATUS_NOT_INIT                        =   0  # Not initialised
+int8   INIT_STATUS_LOCAL_INIT                      =   1  # Locally initialised
+int8   INIT_STATUS_GLOBAL_INIT                     =   2  # Globally initialised
+
+# FpaMeasStatus
+int8   MEAS_STATUS_UNSPECIFIED                     =  -1  # Unspecified
+int8   MEAS_STATUS_NOT_USED                        =   0  # Not used
+int8   MEAS_STATUS_USED                            =   1  # Used
+int8   MEAS_STATUS_DEGRADED                        =   2  # Degraded
+
+# FpaImuStatus
+int8   IMU_STATUS_UNSPECIFIED                      =  -1  # Unspecified
+int8   IMU_STATUS_NOT_CONVERGED                    =   0  # Not converged
+int8   IMU_STATUS_WARMSTARTED                      =   1  # Warmstarted
+int8   IMU_STATUS_ROUGH_CONVERGED                  =   2  # Rough convergence
+int8   IMU_STATUS_FINE_CONVERGED                   =   3  # Fine convergence
+
+# FpaImuNoise
+int8   IMU_NOISE_UNSPECIFIED                       =  -1  # Unspecified
+int8   IMU_NOISE_LOW_NOISE                         =   1  # Low noise
+int8   IMU_NOISE_MEDIUM_NOISE                      =   2  # Medium noise
+int8   IMU_NOISE_HIGH_NOISE                        =   3  # High noise
+int8   IMU_NOISE_RESERVED4                         =   4  # Reserved
+int8   IMU_NOISE_RESERVED5                         =   5  # Reserved
+int8   IMU_NOISE_RESERVED6                         =   6  # Reserved
+int8   IMU_NOISE_RESERVED7                         =   7  # Reserved
+
+# FpaImuConv
+int8   IMU_CONV_UNSPECIFIED                        =  -1  # Unspecified
+int8   IMU_CONV_RESERVED0                          =   0  # Reserved
+int8   IMU_CONV_WAIT_IMU_MEAS                      =   1  # Awaiting IMU measurements
+int8   IMU_CONV_WAIT_GLOBAL_MEAS                   =   2  # Insufficient global measurements
+int8   IMU_CONV_WAIT_MOTION                        =   3  # Insufficient motion
+int8   IMU_CONV_CONVERGING                         =   4  # Converging
+int8   IMU_CONV_RESERVED5                          =   5  # Reserved
+int8   IMU_CONV_RESERVED6                          =   6  # Reserved
+int8   IMU_CONV_IDLE                               =   7  # Idle
+
+# FpaGnssStatus
+int8   GNSS_STATUS_UNSPECIFIED                     =  -1  # Unspecified
+int8   GNSS_STATUS_NO_FIX                          =   0  # No fix
+int8   GNSS_STATUS_SPP                             =   1  # Single-point positioning (SPP)
+int8   GNSS_STATUS_RTK_MB                          =   2  # RTK moving baseline
+int8   GNSS_STATUS_RESERVED3                       =   3  # Reserved
+int8   GNSS_STATUS_RESERVED4                       =   4  # Reserved
+int8   GNSS_STATUS_RTK_FLOAT                       =   5  # RTK float
+int8   GNSS_STATUS_RESERVED6                       =   6  # Reserved
+int8   GNSS_STATUS_RESERVED7                       =   7  # Reserved
+int8   GNSS_STATUS_RTK_FIXED                       =   8  # RTK fixed
+int8   GNSS_STATUS_RESERVED9                       =   9  # Reserved
+
+# FpaBaselineStatus
+int8   BASELINE_STATUS_UNSPECIFIED                 =  -1  # Unspecified
+int8   BASELINE_STATUS_WAITING_FUSION              =   0  # Waiting fusion
+int8   BASELINE_STATUS_NO_FIX                      =   1  # Not available or no fix
+int8   BASELINE_STATUS_FAILING                     =   2  # Failing
+int8   BASELINE_STATUS_PASSING                     =   3  # Passing
+
+# FpaCorrStatus
+int8   CORR_STATUS_UNSPECIFIED                     =  -1  # Unspecified
+int8   CORR_STATUS_WAITING_FUSION                  =   0  # Waiting fusion
+int8   CORR_STATUS_NO_GNSS                         =   1  # No GNSS available
+int8   CORR_STATUS_NO_CORR                         =   2  # No corrections used
+int8   CORR_STATUS_LIMITED_CORR                    =   3  # Limited corrections used: station data & at least one of the constellations (both bands) among the valid rover measurements
+int8   CORR_STATUS_OLD_CORR                        =   4  # Corrections are too old (TBD)
+int8   CORR_STATUS_GOOD_CORR                       =   5  # Sufficient corrections used: station data and corrections for ALL bands among the valid rover measurements
+int8   CORR_STATUS_RESERVED6                       =   6  # Reserved
+int8   CORR_STATUS_RESERVED7                       =   7  # Reserved
+int8   CORR_STATUS_RESERVED8                       =   8  # Reserved
+int8   CORR_STATUS_RESERVED9                       =   9  # Reserved
+
+# FpaCamStatus
+int8   CAM_STATUS_UNSPECIFIED                      =  -1  # Unspecified
+int8   CAM_STATUS_CAM_UNAVL                        =   0  # Camera not available
+int8   CAM_STATUS_BAD_FEAT                         =   1  # Camera available, but not usable  (e.g. too dark)
+int8   CAM_STATUS_RESERVED2                        =   2  # Reserved
+int8   CAM_STATUS_RESERVED3                        =   3  # Reserved
+int8   CAM_STATUS_RESERVED4                        =   4  # Reserved
+int8   CAM_STATUS_GOOD                             =   5  # Camera working and available
+
+# FpaWsStatus
+int8   WS_STATUS_UNSPECIFIED                       =  -1  # Unspecified
+int8   WS_STATUS_NOT_ENABLED                       =   0  # No wheelspeed enabled
+int8   WS_STATUS_MISS_MEAS                         =   1  # Missing wheelspeed measurements
+int8   WS_STATUS_NONE_CONVERGED                    =   2  # At least one wheelspeed enabled, no wheelspeed converged
+int8   WS_STATUS_ONE_CONVERGED                     =   3  # At least one wheelspeed enabled and at least one converged
+int8   WS_STATUS_ALL_CONVERGED                     =   4  # At least one wheelspeed enabled and all converged
+
+# FpaWsConv
+int8   WS_CONV_UNSPECIFIED                         =  -1  # Unspecified
+int8   WS_CONV_WAIT_FUSION                         =   0  # Awaiting Fusion
+int8   WS_CONV_WAIT_WS_MEAS                        =   1  # Missing wheelspeed measurements
+int8   WS_CONV_WAIT_GLOBAL_MEAS                    =   2  # Insufficient global measurements
+int8   WS_CONV_WAIT_MOTION                         =   3  # Insufficient motion
+int8   WS_CONV_WAIT_IMU_BIAS                       =   4  # Insufficient imu bias convergence
+int8   WS_CONV_CONVERGING                          =   5  # Converging
+int8   WS_CONV_IDLE                                =   6  # Idle
+
+# FpaMarkersStatus
+int8   MARKERS_STATUS_UNSPECIFIED                  =  -1  # Unspecified
+int8   MARKERS_STATUS_NOT_ENABLED                  =   0  # No markers available
+int8   MARKERS_STATUS_NONE_CONVERGED               =   1  # Markers available
+int8   MARKERS_STATUS_ONE_CONVERGED                =   2  # Markers available and used
+int8   MARKERS_STATUS_ALL_CONVERGED                =   3  # All markers available and used
+
+# FpaMarkersConv
+int8   MARKERS_CONV_UNSPECIFIED                    =  -1  # Unspecified
+int8   MARKERS_CONV_WAIT_FUSION                    =   0  # Awaiting Fusion
+int8   MARKERS_CONV_WAIT_MARKER_MEAS               =   1  # Waiting marker measurements
+int8   MARKERS_CONV_WAIT_GLOBAL_MEAS               =   2  # Insufficient global measurements
+int8   MARKERS_CONV_CONVERGING                     =   3  # Converging
+int8   MARKERS_CONV_IDLE                           =   4  # Idle
+
+# FpaEpoch
+int8   EPOCH_UNSPECIFIED                           =  -1  # Unspecified
+int8   EPOCH_GNSS1                                 =   1  # GNSS 1
+int8   EPOCH_GNSS2                                 =   2  # GNSS 2
+int8   EPOCH_GNSS                                  =   3  # Combined GNSS
+int8   EPOCH_FUSION                                =   4  # Fusion
+
+# FpaAntState
+int8   ANT_STATE_UNSPECIFIED                       =  -1  # Unspecified
+int8   ANT_STATE_OPEN                              =   0  # No antenna detected (or connected via DC block)
+int8   ANT_STATE_OK                                =   1  # Antenna detected and good
+int8   ANT_STATE_SHORT                             =   2  # Antenna short circuit detected
+
+# FpaAntPower
+int8   ANT_POWER_UNSPECIFIED                       =  -1  # Unspecified
+int8   ANT_POWER_OFF                               =   0  # Antenna power supply is off
+int8   ANT_POWER_ON                                =   1  # Antenna power supply is on
+
+# FpaGnssFix
+int8   GNSS_FIX_UNSPECIFIED                        =  -1  # Unspecified
+int8   GNSS_FIX_UNKNOWN                            =   0  # Unknown
+int8   GNSS_FIX_NOFIX                              =   1  # No fix
+int8   GNSS_FIX_DRONLY                             =   2  # Dead-reckoning only
+int8   GNSS_FIX_TIME                               =   3  # Time-only fix
+int8   GNSS_FIX_S2D                                =   4  # Single 2D fix
+int8   GNSS_FIX_S3D                                =   5  # Single 3D fix
+int8   GNSS_FIX_S3D_DR                             =   6  # Single 3D fix with dead-reckoning
+int8   GNSS_FIX_RTK_FLOAT                          =   7  # RTK float fix
+int8   GNSS_FIX_RTK_FIXED                          =   8  # RTK fixed fix
+
+# FpaTextLevel
+int8   TEXT_LEVEL_UNSPECIFIED                      =  -1  # Unspecified
+int8   TEXT_LEVEL_ERROR                            =   3  # Error
+int8   TEXT_LEVEL_WARNING                          =   4  # Warning
+int8   TEXT_LEVEL_INFO                             =   6  # Info
+int8   TEXT_LEVEL_DEBUG                            =   7  # Debug
+
+# FpaTimebase
+int8   TIMEBASE_UNSPECIFIED                        =  -1  # Unspecified
+int8   TIMEBASE_NONE                               =   0  # None
+int8   TIMEBASE_GNSS                               =   1  # GNSS
+int8   TIMEBASE_UTC                                =   2  # UTC
+
+# FpaTimeref
+int8   TIMEREF_UNSPECIFIED                         =  -1  # Unspecified
+int8   TIMEREF_UTC_NONE                            =  10  # UTC: None (UTC params not yet known)
+int8   TIMEREF_UTC_CRL                             =  11  # UTC: Communications Research Laboratory (CRL), Japan
+int8   TIMEREF_UTC_NIST                            =  12  # UTC: National Institute of Standards and Technology (NIST)
+int8   TIMEREF_UTC_USNO                            =  13  # UTC: U.S. Naval Observatory (USNO)
+int8   TIMEREF_UTC_BIPM                            =  14  # UTC: International Bureau of Weights and Measures (BIPM)
+int8   TIMEREF_UTC_EU                              =  15  # UTC: European laboratories
+int8   TIMEREF_UTC_SU                              =  16  # UTC: Former Soviet Union (SU)
+int8   TIMEREF_UTC_NTSC                            =  17  # UTC: National Time Service Center (NTSC), China
+int8   TIMEREF_GNSS_GPS                            =  21  # GNSS: GPS
+int8   TIMEREF_GNSS_GAL                            =  22  # GNSS: Galileo
+int8   TIMEREF_GNSS_BDS                            =  23  # GNSS: BeiDou
+int8   TIMEREF_GNSS_GLO                            =  24  # GNSS: GLONASS
+int8   TIMEREF_GNSS_NVC                            =  25  # GNSS: NavIC
+int8   TIMEREF_OTHER                               =  99  # other/unknown GNSS/UTC
diff --git a/fixposition_driver_msgs/msg/FpaEoe.msg b/fixposition_driver_msgs/msg/FpaEoe.msg
new file mode 100644
index 0000000..0ea6717
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaEoe.msg
@@ -0,0 +1,9 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-EOE data
+
+std_msgs/Header header
+int8 epoch                 # Epoch identifier, see consts.EPOCH_...
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaGnssant.msg b/fixposition_driver_msgs/msg/FpaGnssant.msg
new file mode 100644
index 0000000..04502f9
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaGnssant.msg
@@ -0,0 +1,14 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-GNSSANT data
+
+std_msgs/Header header
+int8   gnss1_state   # GNSS1 antenna state, see consts.ANT_STATE_...
+int8   gnss1_power   # GNSS1 antenna power, see consts.ANT_POWER_...
+int32  gnss1_age     # Time since gnss1_state or gnss1_power information last changed
+int8   gnss2_state   # GNSS2 antenna state, see consts.ANT_STATE_...
+int8   gnss2_power   # GNSS2 antenna power, see consts.ANT_POWER_...
+int32  gnss2_age     # Time since gnss2_state or gnss2_power information last changed
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaGnsscorr.msg b/fixposition_driver_msgs/msg/FpaGnsscorr.msg
new file mode 100644
index 0000000..c9fd392
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaGnsscorr.msg
@@ -0,0 +1,23 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-GNSSCORR data
+
+std_msgs/Header header
+int8 gnss1_fix          # GNSS1 fix status, see consts.GNSS_FIX_...
+int8 gnss1_nsig_l1      # Number of L1 signals with correction data tracked by GNSS1, -1 if n/a
+int8 gnss1_nsig_l2      # Number of L2 signals with correction data tracked by GNSS1, -1 if n/a
+int8 gnss2_fix          # GNSS2 fix status, see consts.GNSS_FIX_...
+int8 gnss2_nsig_l1      # Number of L1 signals with correction data tracked by GNSS2, -1 if n/a
+int8 gnss2_nsig_l2      # Number of L2 signals with correction data tracked by GNSS2, -1 if n/a
+
+float32 corr_latency       # Average correction data latency (10s window)
+float32 corr_update_rate   # Average correction update rate (10s window)
+float32 corr_data_rate     # Average correction data rate (10s window)
+float32 corr_msg_rate      # Average correction message rate (10s window)
+
+int16 sta_id                    # Correction station ID, range 0–4095, -1 if n/a
+geometry_msgs/Vector3 sta_llh   # Correction station LLH position (latitude, longitude, height)
+int32 sta_dist                  # Correction station baseline length, -1 if n/a
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaImubias.msg b/fixposition_driver_msgs/msg/FpaImubias.msg
new file mode 100644
index 0000000..4ac3ef5
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaImubias.msg
@@ -0,0 +1,16 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-IMUBIAS data
+
+std_msgs/Header header
+int16 fusion_imu                     # Fusion measurement status: IMU, see consts.MEAS_STATUS_...
+int16 imu_status                     # IMU bias status, see consts.IMU_STATUS_...
+int16 imu_noise                      # IMU variance status, see consts.IMU_NOISE_...
+int16 imu_conv                       # IMU convergence status, see consts.IMU_CONV_...
+geometry_msgs/Vector3 bias_acc       # Accelerometer bias
+geometry_msgs/Vector3 bias_gyr       # Gyroscope bias
+geometry_msgs/Vector3 bias_cov_acc   # Accelerometer bias covariance
+geometry_msgs/Vector3 bias_cov_gyr   # Gyroscope bias covariance
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaLlh.msg b/fixposition_driver_msgs/msg/FpaLlh.msg
new file mode 100644
index 0000000..b62d82c
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaLlh.msg
@@ -0,0 +1,10 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-LLH data
+
+std_msgs/Header header
+geometry_msgs/Vector3 position   # LLH position (latitude, longitude, height)
+float64[9] covariance            # Position covariance in ENU
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaOdomenu.msg b/fixposition_driver_msgs/msg/FpaOdomenu.msg
new file mode 100644
index 0000000..f4aa3e5
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaOdomenu.msg
@@ -0,0 +1,19 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-ODOMENU data
+
+std_msgs/Header header
+string pose_frame                            # frame of the pose values [pose, quaternion]
+string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
+geometry_msgs/PoseWithCovariance pose        # position, orientation
+geometry_msgs/TwistWithCovariance velocity   # linear, angular
+geometry_msgs/Vector3 acceleration           # linear acceleration
+
+int8 fusion_status                           # Fusion status, see consts.FUSION_STATUS_LEGACY_...
+int8 imu_bias_status                         # IMU bias status, see consts.IMU_STATUS_LEGACY_...
+int8 gnss1_status                            # GNSS1 status, see consts.GNSS_FIX_...
+int8 gnss2_status                            # GNSS2 status, see consts.GNSS_FIX_...
+int8 wheelspeed_status                       # Wheelspeed status, see consts.WS_STATUS_LEGACY_...
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaOdometry.msg b/fixposition_driver_msgs/msg/FpaOdometry.msg
new file mode 100644
index 0000000..cc28c75
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaOdometry.msg
@@ -0,0 +1,21 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-ODOMETRY data
+
+std_msgs/Header header
+string pose_frame                            # frame of the pose values [pose, quaternion]
+string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
+geometry_msgs/PoseWithCovariance pose        # position, orientation
+geometry_msgs/TwistWithCovariance velocity   # linear, angular
+geometry_msgs/Vector3 acceleration           # linear acceleration
+
+int8 fusion_status                           # Fusion status, see consts.FUSION_STATUS_LEGACY_...
+int8 imu_bias_status                         # IMU bias status, see consts.IMU_STATUS_LEGACY_...
+int8 gnss1_status                            # GNSS1 status, see consts.GNSS_FIX_...
+int8 gnss2_status                            # GNSS2 status, see consts.GNSS_FIX_...
+int8 wheelspeed_status                       # Wheelspeed status, see consts.WS_STATUS_LEGACY_...
+
+string version                               # Fixposition software version
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaOdomsh.msg b/fixposition_driver_msgs/msg/FpaOdomsh.msg
new file mode 100644
index 0000000..9df196c
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaOdomsh.msg
@@ -0,0 +1,19 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-ODOMSH data
+
+std_msgs/Header header
+string pose_frame                            # frame of the pose values [pose, quaternion]
+string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
+geometry_msgs/PoseWithCovariance pose        # position, orientation
+geometry_msgs/TwistWithCovariance velocity   # linear, angular
+geometry_msgs/Vector3 acceleration           # linear acceleration
+
+int8 fusion_status                           # Fusion status, see consts.FUSION_STATUS_LEGACY_...
+int8 imu_bias_status                         # IMU bias status, see consts.IMU_STATUS_LEGACY_...
+int8 gnss1_status                            # GNSS1 status, see consts.GNSS_FIX_...
+int8 gnss2_status                            # GNSS2 status, see consts.GNSS_FIX_...
+int8 wheelspeed_status                       # Wheelspeed status, see consts.WS_STATUS_LEGACY_...
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaOdomstatus.msg b/fixposition_driver_msgs/msg/FpaOdomstatus.msg
new file mode 100644
index 0000000..79c2487
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaOdomstatus.msg
@@ -0,0 +1,28 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-ODOMSTATUS data
+
+std_msgs/Header header
+int8 init_status                             # Fusion initialisation status, see consts.INIT_STATUS_...
+int8 fusion_imu                              # Fusion measurement status: IMU, see consts.MEAS_STATUS_...
+int8 fusion_gnss1                            # Fusion measurement status: GNSS 1, see consts.MEAS_STATUS_...
+int8 fusion_gnss2                            # Fusion measurement status: GNSS 2, see consts.MEAS_STATUS_...
+int8 fusion_corr                             # Fusion measurement status: GNSS corrections, see consts.MEAS_STATUS_...
+int8 fusion_cam1                             # Fusion measurement status: camera, see consts.MEAS_STATUS_...
+int8 fusion_ws                               # Fusion measurement status: wheelspeed, see consts.MEAS_STATUS_...
+int8 fusion_markers                          # Fusion measurement status: markers, see consts.MEAS_STATUS_...
+int8 imu_status                              # IMU bias status, see IMU_STATUS_...
+int8 imu_noise                               # IMU variance status, see consts.IMU_NOISE_...
+int8 imu_conv                                # IMU convergence status, see consts.IMU_CONV_...
+int8 gnss1_status                            # GNSS 1 fix status, see consts.GNSS_STATUS_...
+int8 gnss2_status                            # GNSS 2 fix status, see consts.GNSS_STATUS_...
+int8 baseline_status                         # Baseline status, see consts.BASELINE_STATUS_...
+int8 corr_status                             # GNSS correction status, see consts.CORR_STATUS_...
+int8 cam1_status                             # Camera 1 status, see consts.CAM_STATUS_...
+int8 ws_status                               # Wheelspeed status, see consts.WS_STATUS_...
+int8 ws_conv                                 # Wheelspeed convergence status, see consts.WS_CONV_...
+int8 markers_status                          # Markers status, see consts.MARKERS_STATUS_...
+int8 markers_conv                            # Markers convergence status, see consts.MARKERS_CONV_...
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaText.msg b/fixposition_driver_msgs/msg/FpaText.msg
new file mode 100644
index 0000000..83ee15c
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaText.msg
@@ -0,0 +1,10 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-TEXT data
+
+std_msgs/Header header
+int8   level   # Level, see consts.TEXT_LEVEL_...
+string text    # Text
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/FpaTp.msg b/fixposition_driver_msgs/msg/FpaTp.msg
new file mode 100644
index 0000000..7f6d1c8
--- /dev/null
+++ b/fixposition_driver_msgs/msg/FpaTp.msg
@@ -0,0 +1,14 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# FP_A-TP data
+
+string  tp_name                              # Timepulse name (source)
+string  timebase                             # Time base, see consts.TIMEBASE_...
+string  timeref                              # Time reference, see consts.TIMEREF_...
+int16   tp_week                              # Timepulse week (only available in FP_A-TP version 2)
+int64   tp_tow_sec                           # Timepulse time seconds of week, integer second part (0–604799), or null
+float64 tp_tow_psec                          # Timepulse time seconds of week, sub-second part (0.000000000000–0.999999999999), or null
+int64   gps_leaps                            # GPS leapseconds, or null if unknown
+
+fixposition_driver_msgs/FpaConsts consts # Constants for enums used in FP_A
diff --git a/fixposition_driver_msgs/msg/NmeaConsts.msg b/fixposition_driver_msgs/msg/NmeaConsts.msg
new file mode 100644
index 0000000..876ab37
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaConsts.msg
@@ -0,0 +1,105 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA enum constants
+
+# NmeaTalkerId
+int8   TALKER_ID_UNSPECIFIED                       =  -1  # Unspecified
+int8   TALKER_ID_PROPRIETARY                       =   1  # Proprietary
+int8   TALKER_ID_GPS_SBAS                          =   2  # GPS and/or SBAS
+int8   TALKER_ID_GLO                               =   3  # GLONASS
+int8   TALKER_ID_GAL                               =   4  # GALILEO
+int8   TALKER_ID_BDS                               =   5  # BeiDou
+int8   TALKER_ID_NAVIC                             =   6  # NavIC
+int8   TALKER_ID_QZSS                              =   7  # QZSS
+int8   TALKER_ID_GNSS                              =   8  # GNSS (multi-constellation)
+
+# NmeaQualityGga
+int8   QUALITY_GGA_UNSPECIFIED                     =  -1  # Unspecified
+int8   QUALITY_GGA_NOFIX                           =   0  # No fix
+int8   QUALITY_GGA_SPP                             =   1  # Autonomous GNSS fix
+int8   QUALITY_GGA_DGNSS                           =   2  # Differential GPS fix (e.g. with SBAS)
+int8   QUALITY_GGA_PPS                             =   3  # PPS mode
+int8   QUALITY_GGA_RTK_FIXED                       =   4  # RTK fixed
+int8   QUALITY_GGA_RTK_FLOAT                       =   5  # RTK float
+int8   QUALITY_GGA_ESTIMATED                       =   6  # Estimated (dead reckoning only)
+int8   QUALITY_GGA_MANUAL                          =   7  # Manual input mode
+int8   QUALITY_GGA_SIM                             =   8  # Simulator
+
+# NmeaStatusGllRmc
+int8   STATUS_GLL_RMC_UNSPECIFIED                  =  -1  # Unspecified
+int8   STATUS_GLL_RMC_INVALID                      =   0  # Data invalid
+int8   STATUS_GLL_RMC_VALID                        =   1  # Data valid
+
+# NmeaModeGllVtg
+int8   MODE_GLL_VTG_UNSPECIFIED                    =  -1  # Unspecified
+int8   MODE_GLL_VTG_INVALID                        =   0  # Invalid (no fix)
+int8   MODE_GLL_VTG_AUTONOMOUS                     =   1  # Autonomous mode (SPP)
+int8   MODE_GLL_VTG_DGNSS                          =   2  # Differential GNSS fix
+int8   MODE_GLL_VTG_ESTIMATED                      =   3  # Estimated (dead reckoning only)
+int8   MODE_GLL_VTG_MANUAL                         =   4  # Manual input mode
+int8   MODE_GLL_VTG_SIM                            =   5  # Simulator mode
+
+# NmeaModeRmcGns
+int8   MODE_RMC_GNS_UNSPECIFIED                    =  -1  # Unspecified
+int8   MODE_RMC_GNS_INVALID                        =   0  # Invalid (no fix)
+int8   MODE_RMC_GNS_AUTONOMOUS                     =   1  # Autonomous mode (SPP)
+int8   MODE_RMC_GNS_DGNSS                          =   2  # Differential GNSS fix
+int8   MODE_RMC_GNS_ESTIMATED                      =   3  # Estimated (dead reckoning only)
+int8   MODE_RMC_GNS_RTK_FIXED                      =   4  # RTK fixed
+int8   MODE_RMC_GNS_RTK_FLOAT                      =   5  # RTK float
+int8   MODE_RMC_GNS_PRECISE                        =   6  # Precise
+int8   MODE_RMC_GNS_MANUAL                         =   7  # Manual input mode
+int8   MODE_RMC_GNS_SIM                            =   8  # Simulator mode
+
+# NmeaNavStatusRmc
+int8   NAV_STATUS_RMC_UNSPECIFIED                  =  -1  # Unspecified
+int8   NAV_STATUS_RMC_NA                           =   0  # Equipment does not provide navigational status
+int8   NAV_STATUS_RMC_SAFE                         =   1  # Safe
+int8   NAV_STATUS_RMC_CAUTION                      =   2  # Caution
+int8   NAV_STATUS_RMC_UNSAFE                       =   3  # Unsafe
+
+# NmeaOpModeGsa
+int8   OP_MODE_GSA_UNSPECIFIED                     =  -1  # Unspecified
+int8   OP_MODE_GSA_MANUAL                          =   1  # Manual
+int8   OP_MODE_GSA_AUTO                            =   2  # Automatic
+
+# NmeaNavModeGsa
+int8   NAV_MODE_GSA_UNSPECIFIED                    =  -1  # Unspecified
+int8   NAV_MODE_GSA_NOFIX                          =   1  # No fix
+int8   NAV_MODE_GSA_FIX2D                          =   2  # 2D fix
+int8   NAV_MODE_GSA_FIX3D                          =   3  # 3D fix
+
+# NmeaSystemId
+int8   SYSTEM_ID_UNSPECIFIED                       =  -1  # Unspecified
+int8   SYSTEM_ID_GPS_SBAS                          =   1  # GPS and/or SBAS
+int8   SYSTEM_ID_GLO                               =   2  # GLONASS
+int8   SYSTEM_ID_GAL                               =   3  # Galileo
+int8   SYSTEM_ID_BDS                               =   4  # BeiDou
+int8   SYSTEM_ID_QZSS                              =   5  # QZSS
+int8   SYSTEM_ID_NAVIC                             =   6  # NavIC
+
+# NmeaSignalId
+int8   SIGNAL_ID_UNSPECIFIED                       =  -1  # Unspecified
+int8   SIGNAL_ID_NONE                              =   0  # None
+int8   SIGNAL_ID_GPS_L1CA                          =  11  # GPS/SBAS L1 C/A or SBAS L1 C/A
+int8   SIGNAL_ID_GPS_L2CL                          =  12  # GPS L2 CL
+int8   SIGNAL_ID_GPS_L2CM                          =  13  # GPS L2 CM
+int8   SIGNAL_ID_GPS_L5I                           =  14  # GPS L5 I
+int8   SIGNAL_ID_GPS_L5Q                           =  15  # GPS L5 Q
+int8   SIGNAL_ID_GAL_E1                            =  31  # Galileo E1
+int8   SIGNAL_ID_GAL_E5A                           =  32  # Galileo E5 A
+int8   SIGNAL_ID_GAL_E5B                           =  33  # Galileo E5 B
+int8   SIGNAL_ID_BDS_B1ID                          =  41  # BeiDou B1I D
+int8   SIGNAL_ID_BDS_B2ID                          =  42  # BeiDou B2I D
+int8   SIGNAL_ID_BDS_B1C                           =  43  # BeiDou B1 C
+int8   SIGNAL_ID_BDS_B2A                           =  44  # BeiDou B2 a
+int8   SIGNAL_ID_QZSS_L1CA                         =  51  # QZSS L1 C/A
+int8   SIGNAL_ID_QZSS_L1S                          =  52  # QZSS L1S
+int8   SIGNAL_ID_QZSS_L2CM                         =  53  # QZSS L2 CM
+int8   SIGNAL_ID_QZSS_L2CL                         =  54  # QZSS L2 CL
+int8   SIGNAL_ID_QZSS_L5I                          =  55  # QZSS L5 I
+int8   SIGNAL_ID_QZSS_L5Q                          =  56  # QZSS L5 Q
+int8   SIGNAL_ID_GLO_L1OF                          =  21  # GLONASS L1 OF
+int8   SIGNAL_ID_GLO_L2OF                          =  22  # GLONASS L2 OF
+int8   SIGNAL_ID_NAVIC_L5A                         =  61  # NavIC L5 A
diff --git a/fixposition_driver_msgs/msg/NmeaEpoch.msg b/fixposition_driver_msgs/msg/NmeaEpoch.msg
new file mode 100644
index 0000000..b99b749
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaEpoch.msg
@@ -0,0 +1,64 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA epoch data collected from all NMEA messages
+
+std_msgs/Header header            # Header, timestamp non-zero if date_valid and time_valid
+
+bool     date_valid               # Date is valid
+int16    date_y                   # Date years
+int8     date_m                   # Date months
+int8     date_d                   # Date days
+
+bool     time_valid               # Time is valid
+int8     time_h                   # Time hours
+int8     time_m                   # Time minutes
+float32  time_s                   # Time seconds
+
+int8     status                   # Positioning system status, see consts.STATUS_GLL_RMC_...
+int8     navstatus                # Navigational status (optional), see consts.NAV_STATUS_RMC_...
+int8     mode1                    # Positioning system mode indicator, see consts.MODE_RMC_GNS_...
+int8     mode2                    # Positioning system mode, see consts.MODE_GLL_VTG_...
+int8     quality                  # Fix quality, see consts.QUALITY_GGA_...
+int8     opmode                   # Operation mode, see consts.OP_MODE_GSA_...
+int8     navmode                  # Nav mode, see consts.NAV_MODE_GSA_...
+
+float64  latitude                 # Latitude [deg], NaN if not available
+float64  longitude                # Longitude [deg], NaN if not available
+float64  height                   # Height [m], NaN if not available
+
+int8     num_sv                   # Number of satellites used in solution, -1 if not available
+
+float32  rms_range                # RMS value of the standard deviation of the range inputs to the navigation process, NaN if not available
+float32  std_major                # Standard deviation of semi-major axis of error ellipse, NaN if not available
+float32  std_minor                # Standard deviation of semi-minor axis of error ellipse, NaN if not available
+float32  angle_major              # Angle of semi-major axis of error ellipse from true North, NaN if not available
+float32  std_lat                  # Standard deviation of latitude error, NaN if not available
+float32  std_lon                  # Standard deviation of longitude error, NaN if not available
+float32  std_alt                  # Standard deviation of altitude error, NaN if not available
+
+float32  pdop                     # PDOP, NaN if not available
+float32  hdop                     # PDOP, NaN if not available
+float32  vdop                     # PDOP, NaN if not available
+
+float32  heading                  # True heading [deg], NaN if not available
+float32  speed                    # Speed over ground [knots], NaN if not available
+float32  course                   # Course over ground w.r.t. True North [deg], NaN if not available
+
+float32  cogt                     # Course over ground (true) [deg], NaN if not available
+float32  cogm                     # Course over ground (magnetic) [deg], NaN if not available
+float32  sogn                     # Speed over ground [knots], NaN if not available
+float32  sogk                     # Speed over ground [km/h], NaN if not available
+
+float32  diff_age                 # Differential data age, NaN if not available
+int16    diff_sta                 # Differential station ID
+
+int8     local_hr                 # Local zone hours, always 00 (= UTC)
+int8     local_min                # Local zone minutes, always 00 (= UTC)
+
+fixposition_driver_msgs/NmeaSatellite[] sats  # Satellites
+fixposition_driver_msgs/NmeaSignal[]    sigs  # Signals
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
+
+float64[9] cov_enu  # ENU position covariance, based on std_{lat,lon,alt} or {pdop,hdop}, all 0 if not available
diff --git a/fixposition_driver_msgs/msg/NmeaGga.msg b/fixposition_driver_msgs/msg/NmeaGga.msg
new file mode 100644
index 0000000..628db4c
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaGga.msg
@@ -0,0 +1,25 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-GGA data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+
+bool     time_valid               # Time is valid
+int8     time_h                   # Time hours
+int8     time_m                   # Time minutes
+float32  time_s                   # Time seconds
+
+float64  latitude                 # Latitude [deg], NaN if not available
+float64  longitude                # Longitude [deg], NaN if not available
+float64  height                   # Height [m], NaN if not available
+
+int8     quality                  # Fix quality, see consts.QUALITY_GGA_...
+int8     num_sv                   # Number of satellites used in solution, -1 if not available
+
+float32  hdop                     # Horizontal dilution of precision, NaN if not available
+
+float32  diff_age                 # Differential data age, NaN if not available
+int16    diff_sta                 # Differential station ID
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaGll.msg b/fixposition_driver_msgs/msg/NmeaGll.msg
new file mode 100644
index 0000000..0c57bb8
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaGll.msg
@@ -0,0 +1,19 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-GLL data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+
+bool     time_valid               # Time is valid
+int8     time_h                   # Time hours
+int8     time_m                   # Time minutes
+float32  time_s                   # Time seconds
+
+float64  latitude                 # Latitude [deg], NaN if not available
+float64  longitude                # Longitude [deg], NaN if not available
+
+int8     status                   # Positioning system status, see consts.STATUS_GLL_RMC_...
+int8     mode                     # Positioning system mode, see consts.MODE_GLL_VTG_...
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaGsa.msg b/fixposition_driver_msgs/msg/NmeaGsa.msg
new file mode 100644
index 0000000..b253c4b
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaGsa.msg
@@ -0,0 +1,18 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-GSA data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+int8     system                   # System, see consts.SYSTEM_ID_...
+
+int8[]   sats                     # Satellites
+
+int8     opmode                   # Operation mode, see consts.OP_MODE_GSA_...
+int8     navmode                  # Nav mode, see consts.NAV_MODE_GSA_...
+
+float32  pdop                     # PDOP, NaN if not available
+float32  hdop                     # PDOP, NaN if not available
+float32  vdop                     # PDOP, NaN if not available
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaGst.msg b/fixposition_driver_msgs/msg/NmeaGst.msg
new file mode 100644
index 0000000..215a570
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaGst.msg
@@ -0,0 +1,21 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-GST data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+
+bool     time_valid               # Time is valid
+int8     time_h                   # Time hours
+int8     time_m                   # Time minutes
+float32  time_s                   # Time seconds
+
+float32  rms_range                # RMS value of the standard deviation of the range inputs to the navigation process, NaN if not available
+float32  std_major                # Standard deviation of semi-major axis of error ellipse, NaN if not available
+float32  std_minor                # Standard deviation of semi-minor axis of error ellipse, NaN if not available
+float32  angle_major              # Angle of semi-major axis of error ellipse from true North, NaN if not available
+float32  std_lat                  # Standard deviation of latitude error, NaN if not available
+float32  std_lon                  # Standard deviation of longitude error, NaN if not available
+float32  std_alt                  # Standard deviation of altitude error, NaN if not available
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaGsv.msg b/fixposition_driver_msgs/msg/NmeaGsv.msg
new file mode 100644
index 0000000..47d6d72
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaGsv.msg
@@ -0,0 +1,20 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-GSV data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+int8     system                   # System, see consts.SYSTEM_ID_...
+int8     signal                   # Signal, see consts.SIGNAL_ID_...
+
+int8     num_msgs                 # Number of messages in this GSV sequence (for this system and signal)
+int8     msg_num                  # Message number in sequence (1...num_msgs)
+
+int8[]   azel_sat                 # Satellite for azimuth/elevation
+int16[]  az                       # Azimuth [deg]
+int16[]  el                       # Elevations [deg]
+
+int8[]   cno_sat                  # Satellite for signal level
+float32[] cno                     # Signal level [dbHz]
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaHdt.msg b/fixposition_driver_msgs/msg/NmeaHdt.msg
new file mode 100644
index 0000000..42dfa4c
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaHdt.msg
@@ -0,0 +1,10 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-HDT data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+
+float32  heading                  # True heading [deg], NaN if not available
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaRmc.msg b/fixposition_driver_msgs/msg/NmeaRmc.msg
new file mode 100644
index 0000000..0e91201
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaRmc.msg
@@ -0,0 +1,29 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-RMC data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+
+bool     date_valid               # Date is valid
+int16    date_y                   # Date years
+int8     date_m                   # Date months
+int8     date_d                   # Date days
+
+bool     time_valid               # Time is valid
+int8     time_h                   # Time hours
+int8     time_m                   # Time minutes
+float32  time_s                   # Time seconds
+
+int8     status                   # Positioning system status, see consts.STATUS_GLL_RMC_...
+int8     mode                     # Positioning system mode indicator, see consts.MODE_RMC_GNS_...
+int8     navstatus                # Navigational status (optional), see consts.NAV_STATUS_RMC_...
+
+float64  latitude                 # Latitude [deg], NaN if not available
+float64  longitude                # Longitude [deg], NaN if not available
+float64  height                   # Height [m], NaN if not available
+
+float32  speed                    # Speed over ground [knots], NaN if not available
+float32  course                   # Course over ground w.r.t. True North [deg], NaN if not available
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaSatellite.msg b/fixposition_driver_msgs/msg/NmeaSatellite.msg
new file mode 100644
index 0000000..3c7af66
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaSatellite.msg
@@ -0,0 +1,11 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA satellite info
+
+int8     system                   # System, see consts.SYSTEM_ID_...
+int8     svid                     # Satellite
+int16    az                       # Azimuth [deg]
+int16    el                       # Elevations [deg]
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaSignal.msg b/fixposition_driver_msgs/msg/NmeaSignal.msg
new file mode 100644
index 0000000..811a5e1
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaSignal.msg
@@ -0,0 +1,12 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA signal info
+
+int8     system                   # System, see consts.SYSTEM_ID_...
+int8     svid                     # Satellite
+int8     signal                   # Signal, see consts.SIGNAL_ID_...
+float32  cno                      # Signal level [dbHz]
+bool     used                     # Signal used
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaVtg.msg b/fixposition_driver_msgs/msg/NmeaVtg.msg
new file mode 100644
index 0000000..b3ba3f4
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaVtg.msg
@@ -0,0 +1,15 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-VTG data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+
+float32  cogt                     # Course over ground (true) [deg], NaN if not available
+float32  cogm                     # Course over ground (magnetic) [deg], NaN if not available
+float32  sogn                     # Speed over ground [knots], NaN if not available
+float32  sogk                     # Speed over ground [km/h], NaN if not available
+
+int8     mode                     # Positioning system mode, see consts.MODE_GLL_VTG_...
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/NmeaZda.msg b/fixposition_driver_msgs/msg/NmeaZda.msg
new file mode 100644
index 0000000..184c6cd
--- /dev/null
+++ b/fixposition_driver_msgs/msg/NmeaZda.msg
@@ -0,0 +1,21 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# NMEA-Gx-ZDA data
+
+int8     talker                   # Talker, see consts.TALKER_ID_...
+
+bool     date_valid               # Date is valid
+int16    date_y                   # Date years
+int8     date_m                   # Date months
+int8     date_d                   # Date days
+
+bool     time_valid               # Time is valid
+int8     time_h                   # Time hours
+int8     time_m                   # Time minutes
+float32  time_s                   # Time seconds
+
+int8     local_hr                 # Local zone hours, always 00 (= UTC)
+int8     local_min                # Local zone minutes, always 00 (= UTC)
+
+fixposition_driver_msgs/NmeaConsts consts # Constants for enums used in NMEA
diff --git a/fixposition_driver_msgs/msg/ParserMsg.msg b/fixposition_driver_msgs/msg/ParserMsg.msg
new file mode 100644
index 0000000..f95a47f
--- /dev/null
+++ b/fixposition_driver_msgs/msg/ParserMsg.msg
@@ -0,0 +1,23 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# Similar to a fpsdk::common::parser::ParserMsg
+#
+# !!! fpsdk_{ros1,ros2}/msg/ParserMsg.msg should be kept in sync !!!
+
+std_msgs/Header header            # Timestamp of message reception time
+int8    protocol                  # Protocol
+int8    PROTOCOL_UNSPECIFIED = 0
+int8    PROTOCOL_FP_A        = 1
+int8    PROTOCOL_FP_B        = 2
+int8    PROTOCOL_NMEA        = 3
+int8    PROTOCOL_UBX         = 4
+int8    PROTOCOL_RTCM3       = 5
+int8    PROTOCOL_UNI_B       = 6
+int8    PROTOCOL_NOV_B       = 7
+int8    PROTOCOL_SPARTN      = 8
+int8    PROTOCOL_OTHER       = 9
+uint8[] data                      # Message data
+string  name                      # Message name
+uint64  seq                       # Message counter
+string  info                      # Message (debug) info
diff --git a/fixposition_driver_msgs/msg/Speed.msg b/fixposition_driver_msgs/msg/Speed.msg
new file mode 100644
index 0000000..5fbb7a6
--- /dev/null
+++ b/fixposition_driver_msgs/msg/Speed.msg
@@ -0,0 +1,7 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# Wheel speed input to Fixposition ROS driver
+
+# Velocity values for one or more WheelSensors
+fixposition_driver_msgs/WheelSensor[] sensors
diff --git a/fixposition_driver_msgs/msg/WheelSensor.msg b/fixposition_driver_msgs/msg/WheelSensor.msg
new file mode 100644
index 0000000..d2a7f5d
--- /dev/null
+++ b/fixposition_driver_msgs/msg/WheelSensor.msg
@@ -0,0 +1,20 @@
+# Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+# License: see the LICENSE file
+#
+# Individual wheelspeed measurement
+
+# Standard metadata
+std_msgs/Header header
+
+# Location of the wheelspeed measurement (one of: RC, FR, FL, RR, RL)
+string location
+
+# Velocity values in [mm/s]
+int32 vx
+int32 vy
+int32 vz
+
+# Velocity validity
+bool vx_valid
+bool vy_valid
+bool vz_valid
diff --git a/fixposition_driver_msgs/package.xml b/fixposition_driver_msgs/package.xml
new file mode 100644
index 0000000..3c2ae65
--- /dev/null
+++ b/fixposition_driver_msgs/package.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package format="3">
+    <name>fixposition_driver_msgs</name>
+    <version>8.0.0</version>
+    <description>Fixposition ROS driver messages</description>
+    <maintainer email="support@fixposition.com">Fixposition Support</maintainer>
+    <license>MIT</license>
+
+    <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+    <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+    <buildtool_depend condition="$ROS_VERSION == 2">rosidl_default_generators</buildtool_depend>
+
+    <build_depend>ros_environment</build_depend>
+    <build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
+    <build_depend condition="$ROS_VERSION == 2">builtin_interfaces</build_depend>
+
+    <depend>std_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>nav_msgs</depend>
+    <depend>sensor_msgs</depend>
+    <depend>fixposition_driver_lib</depend>
+    <depend>fpsdk_common</depend>
+    <depend condition="$ROS_VERSION == 1">fpsdk_ros1</depend>
+    <depend condition="$ROS_VERSION == 2">fpsdk_ros2</depend>
+
+    <exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
+    <exec_depend condition="$ROS_VERSION == 2">rosidl_default_runtime</exec_depend>
+
+    <member_of_group condition="$ROS_VERSION == 2">rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type condition="$ROS_VERSION == 1">catkin</build_type>
+        <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+    </export>
+</package>
diff --git a/fixposition_driver_msgs/src/data_to_ros.cpp b/fixposition_driver_msgs/src/data_to_ros.cpp
new file mode 100644
index 0000000..20d35cb
--- /dev/null
+++ b/fixposition_driver_msgs/src/data_to_ros.cpp
@@ -0,0 +1,25 @@
+/**
+ * \verbatim
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
+ * \endverbatim
+ *
+ * @file
+ * @brief Helpers to convert data to ROS 1 and 2 msgs
+ */
+
+/* LIBC/STL */
+
+/* EXTERNAL */
+
+/* PACKAGE */
+#include "fixposition_driver_msgs/data_to_ros.hpp"
+
+namespace fixposition {
+/* ****************************************************************************************************************** */
+
+/* ****************************************************************************************************************** */
+}  // namespace fixposition
diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt
index a002a20..0d9b040 100644
--- a/fixposition_driver_ros1/CMakeLists.txt
+++ b/fixposition_driver_ros1/CMakeLists.txt
@@ -1,17 +1,29 @@
 # GENERAL ==============================================================================================================
-cmake_minimum_required(VERSION 3.5)
+
+cmake_minimum_required(VERSION 3.16)
 project(fixposition_driver_ros1 LANGUAGES CXX)
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic\
-    -Wshadow -Wunused-parameter -Wformat -Wpointer-arith")
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror\
+    -Wshadow -Wunused-parameter -Wformat -Wpointer-arith -Woverloaded-virtual")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release)
+endif()
+if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+    add_compile_definitions(NDEBUG)
+endif()
+
 
 # DEPENDENCIES =========================================================================================================
+
 find_package(Boost 1.65.0 REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(fixposition_driver_lib REQUIRED)
+find_package(fixposition_driver_msgs REQUIRED)
+find_package(fpsdk_common REQUIRED)
+find_package(fpsdk_ros1 REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   tf
@@ -24,59 +36,28 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   tf2_geometry_msgs
 )
-add_message_files(
-  FILES
-  Speed.msg
-  WheelSensor.msg
-  GnssSats.msg
-  NMEA.msg
-  fpa/eoe.msg
-  fpa/gnssant.msg
-  fpa/gnsscorr.msg
-  fpa/imubias.msg
-  fpa/llh.msg
-  fpa/odomenu.msg
-  fpa/odometry.msg
-  fpa/odomsh.msg
-  fpa/odomstatus.msg
-  fpa/text.msg
-  fpa/tp.msg
-  nmea/gpgga.msg
-  nmea/gpgll.msg
-  nmea/gngsa.msg
-  nmea/gpgst.msg
-  nmea/gxgsv.msg
-  nmea/gphdt.msg
-  nmea/gprmc.msg
-  nmea/gpvtg.msg
-  nmea/gpzda.msg
-  extras/CovWarn.msg
-)
-
-generate_messages(
-  DEPENDENCIES
-  std_msgs
-  nav_msgs
-  geometry_msgs
-)
 
 catkin_package(
   INCLUDE_DIRS include
   CATKIN_DEPENDS
-  tf tf2_ros
-  roscpp geometry_msgs tf2_geometry_msgs
-  nav_msgs std_msgs message_runtime
+    tf tf2_ros
+    roscpp geometry_msgs tf2_geometry_msgs
+    nav_msgs std_msgs message_runtime
 )
 
 include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${fixposition_driver_lib_INCLUDE_DIRS}
+  ${fixposition_driver_msgs_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIR}
   ${Boost_INCLUDE_DIR}
+  ${fpsdk_common_INLCUDE_DIRS} ${fpsdk_ros1_INLCUDE_DIRS}
 )
 
+
 # BUILD EXECUTABLE =====================================================================================================
+
 add_executable(
   ${PROJECT_NAME}
   src/fixposition_driver_node.cpp
@@ -89,12 +70,13 @@ target_link_libraries(
   ${catkin_LIBRARIES}
   ${fixposition_driver_lib_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${fpsdk_common_LIBRARIES} ${fpsdk_ros1_LIBRARIES}
   pthread
 )
 
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp)
 
 # INSTALL ==============================================================================================================
+
 install(DIRECTORY include/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 )
diff --git a/fixposition_driver_ros1/Doxyfile b/fixposition_driver_ros1/Doxyfile
deleted file mode 100644
index 5fdd3e8..0000000
--- a/fixposition_driver_ros1/Doxyfile
+++ /dev/null
@@ -1,2427 +0,0 @@
-# Doxyfile 1.8.11
-
-# This file describes the settings to be used by the documentation system
-# doxygen (www.doxygen.org) for a project.
-#
-# All text after a double hash (##) is considered a comment and is placed in
-# front of the TAG it is preceding.
-#
-# All text after a single hash (#) is considered a comment and will be ignored.
-# The format is:
-# TAG = value [value, ...]
-# For lists, items can also be appended using:
-# TAG += value [value, ...]
-# Values that contain spaces should be placed between quotes (\" \").
-
-#---------------------------------------------------------------------------
-# Project related configuration options
-#---------------------------------------------------------------------------
-
-# This tag specifies the encoding used for all characters in the config file
-# that follow. The default is UTF-8 which is also the encoding used for all text
-# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
-# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
-# for the list of possible encodings.
-# The default value is: UTF-8.
-
-DOXYFILE_ENCODING      = UTF-8
-
-# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
-# double-quotes, unless you are using Doxywizard) that should identify the
-# project for which the documentation is generated. This name is used in the
-# title of most generated pages and in a few other places.
-# The default value is: My Project.
-
-PROJECT_NAME           = "Fixposition Driver ROS1"
-
-# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
-# could be handy for archiving the generated documentation or if some version
-# control system is used.
-
-PROJECT_NUMBER         =
-
-# Using the PROJECT_BRIEF tag one can provide an optional one line description
-# for a project that appears at the top of each page and should give viewer a
-# quick idea about the purpose of the project. Keep the description short.
-
-PROJECT_BRIEF          =
-
-# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
-# in the documentation. The maximum height of the logo should not exceed 55
-# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
-# the logo to the output directory.
-
-PROJECT_LOGO           =
-
-# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
-# into which the generated documentation will be written. If a relative path is
-# entered, it will be relative to the location where doxygen was started. If
-# left blank the current directory will be used.
-
-OUTPUT_DIRECTORY       = doc_gen
-
-# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
-# directories (in 2 levels) under the output directory of each output format and
-# will distribute the generated files over these directories. Enabling this
-# option can be useful when feeding doxygen a huge amount of source files, where
-# putting all generated files in the same directory would otherwise causes
-# performance problems for the file system.
-# The default value is: NO.
-
-CREATE_SUBDIRS         = NO
-
-# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
-# characters to appear in the names of generated files. If set to NO, non-ASCII
-# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
-# U+3044.
-# The default value is: NO.
-
-ALLOW_UNICODE_NAMES    = NO
-
-# The OUTPUT_LANGUAGE tag is used to specify the language in which all
-# documentation generated by doxygen is written. Doxygen will use this
-# information to generate all constant output in the proper language.
-# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
-# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
-# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
-# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
-# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
-# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
-# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
-# Ukrainian and Vietnamese.
-# The default value is: English.
-
-OUTPUT_LANGUAGE        = English
-
-# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
-# descriptions after the members that are listed in the file and class
-# documentation (similar to Javadoc). Set to NO to disable this.
-# The default value is: YES.
-
-BRIEF_MEMBER_DESC      = YES
-
-# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
-# description of a member or function before the detailed description
-#
-# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
-# brief descriptions will be completely suppressed.
-# The default value is: YES.
-
-REPEAT_BRIEF           = YES
-
-# This tag implements a quasi-intelligent brief description abbreviator that is
-# used to form the text in various listings. Each string in this list, if found
-# as the leading text of the brief description, will be stripped from the text
-# and the result, after processing the whole list, is used as the annotated
-# text. Otherwise, the brief description is used as-is. If left blank, the
-# following values are used ($name is automatically replaced with the name of
-# the entity):The $name class, The $name widget, The $name file, is, provides,
-# specifies, contains, represents, a, an and the.
-
-ABBREVIATE_BRIEF       =
-
-# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
-# doxygen will generate a detailed section even if there is only a brief
-# description.
-# The default value is: NO.
-
-ALWAYS_DETAILED_SEC    = NO
-
-# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
-# inherited members of a class in the documentation of that class as if those
-# members were ordinary class members. Constructors, destructors and assignment
-# operators of the base classes will not be shown.
-# The default value is: NO.
-
-INLINE_INHERITED_MEMB  = NO
-
-# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
-# before files name in the file list and in the header files. If set to NO the
-# shortest path that makes the file name unique will be used
-# The default value is: YES.
-
-FULL_PATH_NAMES        = YES
-
-# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
-# Stripping is only done if one of the specified strings matches the left-hand
-# part of the path. The tag can be used to show relative paths in the file list.
-# If left blank the directory from which doxygen is run is used as the path to
-# strip.
-#
-# Note that you can specify absolute paths here, but also relative paths, which
-# will be relative from the directory where doxygen is started.
-# This tag requires that the tag FULL_PATH_NAMES is set to YES.
-
-STRIP_FROM_PATH        =
-
-# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
-# path mentioned in the documentation of a class, which tells the reader which
-# header file to include in order to use a class. If left blank only the name of
-# the header file containing the class definition is used. Otherwise one should
-# specify the list of include paths that are normally passed to the compiler
-# using the -I flag.
-
-STRIP_FROM_INC_PATH    =
-
-# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
-# less readable) file names. This can be useful is your file systems doesn't
-# support long names like on DOS, Mac, or CD-ROM.
-# The default value is: NO.
-
-SHORT_NAMES            = NO
-
-# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
-# first line (until the first dot) of a Javadoc-style comment as the brief
-# description. If set to NO, the Javadoc-style will behave just like regular Qt-
-# style comments (thus requiring an explicit @brief command for a brief
-# description.)
-# The default value is: NO.
-
-JAVADOC_AUTOBRIEF      = NO
-
-# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
-# line (until the first dot) of a Qt-style comment as the brief description. If
-# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
-# requiring an explicit \brief command for a brief description.)
-# The default value is: NO.
-
-QT_AUTOBRIEF           = NO
-
-# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
-# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
-# a brief description. This used to be the default behavior. The new default is
-# to treat a multi-line C++ comment block as a detailed description. Set this
-# tag to YES if you prefer the old behavior instead.
-#
-# Note that setting this tag to YES also means that rational rose comments are
-# not recognized any more.
-# The default value is: NO.
-
-MULTILINE_CPP_IS_BRIEF = NO
-
-# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
-# documentation from any documented member that it re-implements.
-# The default value is: YES.
-
-INHERIT_DOCS           = YES
-
-# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
-# page for each member. If set to NO, the documentation of a member will be part
-# of the file/class/namespace that contains it.
-# The default value is: NO.
-
-SEPARATE_MEMBER_PAGES  = NO
-
-# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
-# uses this value to replace tabs by spaces in code fragments.
-# Minimum value: 1, maximum value: 16, default value: 4.
-
-TAB_SIZE               = 4
-
-# This tag can be used to specify a number of aliases that act as commands in
-# the documentation. An alias has the form:
-# name=value
-# For example adding
-# "sideeffect=@par Side Effects:\n"
-# will allow you to put the command \sideeffect (or @sideeffect) in the
-# documentation, which will result in a user-defined paragraph with heading
-# "Side Effects:". You can put \n's in the value part of an alias to insert
-# newlines.
-
-ALIASES                =
-
-# This tag can be used to specify a number of word-keyword mappings (TCL only).
-# A mapping has the form "name=value". For example adding "class=itcl::class"
-# will allow you to use the command class in the itcl::class meaning.
-
-TCL_SUBST              =
-
-# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
-# only. Doxygen will then generate output that is more tailored for C. For
-# instance, some of the names that are used will be different. The list of all
-# members will be omitted, etc.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_FOR_C  = NO
-
-# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
-# Python sources only. Doxygen will then generate output that is more tailored
-# for that language. For instance, namespaces will be presented as packages,
-# qualified scopes will look different, etc.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_JAVA   = NO
-
-# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
-# sources. Doxygen will then generate output that is tailored for Fortran.
-# The default value is: NO.
-
-OPTIMIZE_FOR_FORTRAN   = NO
-
-# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
-# sources. Doxygen will then generate output that is tailored for VHDL.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_VHDL   = NO
-
-# Doxygen selects the parser to use depending on the extension of the files it
-# parses. With this tag you can assign which parser to use for a given
-# extension. Doxygen has a built-in mapping, but you can override or extend it
-# using this tag. The format is ext=language, where ext is a file extension, and
-# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
-# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
-# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
-# Fortran. In the later case the parser tries to guess whether the code is fixed
-# or free formatted code, this is the default for Fortran type files), VHDL. For
-# instance to make doxygen treat .inc files as Fortran files (default is PHP),
-# and .f files as C (default is Fortran), use: inc=Fortran f=C.
-#
-# Note: For files without extension you can use no_extension as a placeholder.
-#
-# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
-# the files are not read by doxygen.
-
-EXTENSION_MAPPING      =
-
-# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
-# according to the Markdown format, which allows for more readable
-# documentation. See http://daringfireball.net/projects/markdown/ for details.
-# The output of markdown processing is further processed by doxygen, so you can
-# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
-# case of backward compatibilities issues.
-# The default value is: YES.
-
-MARKDOWN_SUPPORT       = YES
-
-# When enabled doxygen tries to link words that correspond to documented
-# classes, or namespaces to their corresponding documentation. Such a link can
-# be prevented in individual cases by putting a % sign in front of the word or
-# globally by setting AUTOLINK_SUPPORT to NO.
-# The default value is: YES.
-
-AUTOLINK_SUPPORT       = YES
-
-# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
-# to include (a tag file for) the STL sources as input, then you should set this
-# tag to YES in order to let doxygen match functions declarations and
-# definitions whose arguments contain STL classes (e.g. func(std::string);
-# versus func(std::string) {}). This also make the inheritance and collaboration
-# diagrams that involve STL classes more complete and accurate.
-# The default value is: NO.
-
-BUILTIN_STL_SUPPORT    = NO
-
-# If you use Microsoft's C++/CLI language, you should set this option to YES to
-# enable parsing support.
-# The default value is: NO.
-
-CPP_CLI_SUPPORT        = NO
-
-# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
-# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
-# will parse them like normal C++ but will assume all classes use public instead
-# of private inheritance when no explicit protection keyword is present.
-# The default value is: NO.
-
-SIP_SUPPORT            = NO
-
-# For Microsoft's IDL there are propget and propput attributes to indicate
-# getter and setter methods for a property. Setting this option to YES will make
-# doxygen to replace the get and set methods by a property in the documentation.
-# This will only work if the methods are indeed getting or setting a simple
-# type. If this is not the case, or you want to show the methods anyway, you
-# should set this option to NO.
-# The default value is: YES.
-
-IDL_PROPERTY_SUPPORT   = YES
-
-# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
-# tag is set to YES then doxygen will reuse the documentation of the first
-# member in the group (if any) for the other members of the group. By default
-# all members of a group must be documented explicitly.
-# The default value is: NO.
-
-DISTRIBUTE_GROUP_DOC   = NO
-
-# If one adds a struct or class to a group and this option is enabled, then also
-# any nested class or struct is added to the same group. By default this option
-# is disabled and one has to add nested compounds explicitly via \ingroup.
-# The default value is: NO.
-
-GROUP_NESTED_COMPOUNDS = NO
-
-# Set the SUBGROUPING tag to YES to allow class member groups of the same type
-# (for instance a group of public functions) to be put as a subgroup of that
-# type (e.g. under the Public Functions section). Set it to NO to prevent
-# subgrouping. Alternatively, this can be done per class using the
-# \nosubgrouping command.
-# The default value is: YES.
-
-SUBGROUPING            = YES
-
-# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
-# are shown inside the group in which they are included (e.g. using \ingroup)
-# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
-# and RTF).
-#
-# Note that this feature does not work in combination with
-# SEPARATE_MEMBER_PAGES.
-# The default value is: NO.
-
-INLINE_GROUPED_CLASSES = NO
-
-# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
-# with only public data fields or simple typedef fields will be shown inline in
-# the documentation of the scope in which they are defined (i.e. file,
-# namespace, or group documentation), provided this scope is documented. If set
-# to NO, structs, classes, and unions are shown on a separate page (for HTML and
-# Man pages) or section (for LaTeX and RTF).
-# The default value is: NO.
-
-INLINE_SIMPLE_STRUCTS  = NO
-
-# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
-# enum is documented as struct, union, or enum with the name of the typedef. So
-# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
-# with name TypeT. When disabled the typedef will appear as a member of a file,
-# namespace, or class. And the struct will be named TypeS. This can typically be
-# useful for C code in case the coding convention dictates that all compound
-# types are typedef'ed and only the typedef is referenced, never the tag name.
-# The default value is: NO.
-
-TYPEDEF_HIDES_STRUCT   = NO
-
-# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
-# cache is used to resolve symbols given their name and scope. Since this can be
-# an expensive process and often the same symbol appears multiple times in the
-# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
-# doxygen will become slower. If the cache is too large, memory is wasted. The
-# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
-# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
-# symbols. At the end of a run doxygen will report the cache usage and suggest
-# the optimal cache size from a speed point of view.
-# Minimum value: 0, maximum value: 9, default value: 0.
-
-LOOKUP_CACHE_SIZE      = 0
-
-#---------------------------------------------------------------------------
-# Build related configuration options
-#---------------------------------------------------------------------------
-
-# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
-# documentation are documented, even if no documentation was available. Private
-# class members and static file members will be hidden unless the
-# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
-# Note: This will also disable the warnings about undocumented members that are
-# normally produced when WARNINGS is set to YES.
-# The default value is: NO.
-
-EXTRACT_ALL            =YES
-
-# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
-# be included in the documentation.
-# The default value is: NO.
-
-EXTRACT_PRIVATE        = YES
-
-# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
-# scope will be included in the documentation.
-# The default value is: NO.
-
-EXTRACT_PACKAGE        = NO
-
-# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
-# included in the documentation.
-# The default value is: NO.
-
-EXTRACT_STATIC         = YES
-
-# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
-# locally in source files will be included in the documentation. If set to NO,
-# only classes defined in header files are included. Does not have any effect
-# for Java sources.
-# The default value is: YES.
-
-EXTRACT_LOCAL_CLASSES  = YES
-
-# This flag is only useful for Objective-C code. If set to YES, local methods,
-# which are defined in the implementation section but not in the interface are
-# included in the documentation. If set to NO, only methods in the interface are
-# included.
-# The default value is: NO.
-
-EXTRACT_LOCAL_METHODS  = NO
-
-# If this flag is set to YES, the members of anonymous namespaces will be
-# extracted and appear in the documentation as a namespace called
-# 'anonymous_namespace{file}', where file will be replaced with the base name of
-# the file that contains the anonymous namespace. By default anonymous namespace
-# are hidden.
-# The default value is: NO.
-
-EXTRACT_ANON_NSPACES   = NO
-
-# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
-# undocumented members inside documented classes or files. If set to NO these
-# members will be included in the various overviews, but no documentation
-# section is generated. This option has no effect if EXTRACT_ALL is enabled.
-# The default value is: NO.
-
-HIDE_UNDOC_MEMBERS     = NO
-
-# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
-# undocumented classes that are normally visible in the class hierarchy. If set
-# to NO, these classes will be included in the various overviews. This option
-# has no effect if EXTRACT_ALL is enabled.
-# The default value is: NO.
-
-HIDE_UNDOC_CLASSES     = NO
-
-# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
-# (class|struct|union) declarations. If set to NO, these declarations will be
-# included in the documentation.
-# The default value is: NO.
-
-HIDE_FRIEND_COMPOUNDS  = NO
-
-# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
-# documentation blocks found inside the body of a function. If set to NO, these
-# blocks will be appended to the function's detailed documentation block.
-# The default value is: NO.
-
-HIDE_IN_BODY_DOCS      = NO
-
-# The INTERNAL_DOCS tag determines if documentation that is typed after a
-# \internal command is included. If the tag is set to NO then the documentation
-# will be excluded. Set it to YES to include the internal documentation.
-# The default value is: NO.
-
-INTERNAL_DOCS          = NO
-
-# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
-# names in lower-case letters. If set to YES, upper-case letters are also
-# allowed. This is useful if you have classes or files whose names only differ
-# in case and if your file system supports case sensitive file names. Windows
-# and Mac users are advised to set this option to NO.
-# The default value is: system dependent.
-
-CASE_SENSE_NAMES       = YES
-
-# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
-# their full class and namespace scopes in the documentation. If set to YES, the
-# scope will be hidden.
-# The default value is: NO.
-
-HIDE_SCOPE_NAMES       = NO
-
-# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
-# append additional text to a page's title, such as Class Reference. If set to
-# YES the compound reference will be hidden.
-# The default value is: NO.
-
-HIDE_COMPOUND_REFERENCE= NO
-
-# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
-# the files that are included by a file in the documentation of that file.
-# The default value is: YES.
-
-SHOW_INCLUDE_FILES     = YES
-
-# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
-# grouped member an include statement to the documentation, telling the reader
-# which file to include in order to use the member.
-# The default value is: NO.
-
-SHOW_GROUPED_MEMB_INC  = NO
-
-# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
-# files with double quotes in the documentation rather than with sharp brackets.
-# The default value is: NO.
-
-FORCE_LOCAL_INCLUDES   = NO
-
-# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
-# documentation for inline members.
-# The default value is: YES.
-
-INLINE_INFO            = YES
-
-# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
-# (detailed) documentation of file and class members alphabetically by member
-# name. If set to NO, the members will appear in declaration order.
-# The default value is: YES.
-
-SORT_MEMBER_DOCS       = YES
-
-# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
-# descriptions of file, namespace and class members alphabetically by member
-# name. If set to NO, the members will appear in declaration order. Note that
-# this will also influence the order of the classes in the class list.
-# The default value is: NO.
-
-SORT_BRIEF_DOCS        = NO
-
-# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
-# (brief and detailed) documentation of class members so that constructors and
-# destructors are listed first. If set to NO the constructors will appear in the
-# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
-# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
-# member documentation.
-# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
-# detailed member documentation.
-# The default value is: NO.
-
-SORT_MEMBERS_CTORS_1ST = NO
-
-# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
-# of group names into alphabetical order. If set to NO the group names will
-# appear in their defined order.
-# The default value is: NO.
-
-SORT_GROUP_NAMES       = NO
-
-# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
-# fully-qualified names, including namespaces. If set to NO, the class list will
-# be sorted only by class name, not including the namespace part.
-# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
-# Note: This option applies only to the class list, not to the alphabetical
-# list.
-# The default value is: NO.
-
-SORT_BY_SCOPE_NAME     = NO
-
-# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
-# type resolution of all parameters of a function it will reject a match between
-# the prototype and the implementation of a member function even if there is
-# only one candidate or it is obvious which candidate to choose by doing a
-# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
-# accept a match between prototype and implementation in such cases.
-# The default value is: NO.
-
-STRICT_PROTO_MATCHING  = NO
-
-# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
-# list. This list is created by putting \todo commands in the documentation.
-# The default value is: YES.
-
-GENERATE_TODOLIST      = YES
-
-# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
-# list. This list is created by putting \test commands in the documentation.
-# The default value is: YES.
-
-GENERATE_TESTLIST      = YES
-
-# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
-# list. This list is created by putting \bug commands in the documentation.
-# The default value is: YES.
-
-GENERATE_BUGLIST       = YES
-
-# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
-# the deprecated list. This list is created by putting \deprecated commands in
-# the documentation.
-# The default value is: YES.
-
-GENERATE_DEPRECATEDLIST= YES
-
-# The ENABLED_SECTIONS tag can be used to enable conditional documentation
-# sections, marked by \if <section_label> ... \endif and \cond <section_label>
-# ... \endcond blocks.
-
-ENABLED_SECTIONS       =
-
-# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
-# initial value of a variable or macro / define can have for it to appear in the
-# documentation. If the initializer consists of more lines than specified here
-# it will be hidden. Use a value of 0 to hide initializers completely. The
-# appearance of the value of individual variables and macros / defines can be
-# controlled using \showinitializer or \hideinitializer command in the
-# documentation regardless of this setting.
-# Minimum value: 0, maximum value: 10000, default value: 30.
-
-MAX_INITIALIZER_LINES  = 30
-
-# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
-# the bottom of the documentation of classes and structs. If set to YES, the
-# list will mention the files that were used to generate the documentation.
-# The default value is: YES.
-
-SHOW_USED_FILES        = YES
-
-# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
-# will remove the Files entry from the Quick Index and from the Folder Tree View
-# (if specified).
-# The default value is: YES.
-
-SHOW_FILES             = YES
-
-# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
-# page. This will remove the Namespaces entry from the Quick Index and from the
-# Folder Tree View (if specified).
-# The default value is: YES.
-
-SHOW_NAMESPACES        = YES
-
-# The FILE_VERSION_FILTER tag can be used to specify a program or script that
-# doxygen should invoke to get the current version for each file (typically from
-# the version control system). Doxygen will invoke the program by executing (via
-# popen()) the command command input-file, where command is the value of the
-# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
-# by doxygen. Whatever the program writes to standard output is used as the file
-# version. For an example see the documentation.
-
-FILE_VERSION_FILTER    =
-
-# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
-# by doxygen. The layout file controls the global structure of the generated
-# output files in an output format independent way. To create the layout file
-# that represents doxygen's defaults, run doxygen with the -l option. You can
-# optionally specify a file name after the option, if omitted DoxygenLayout.xml
-# will be used as the name of the layout file.
-#
-# Note that if you run doxygen from a directory containing a file called
-# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
-# tag is left empty.
-
-LAYOUT_FILE            =
-
-# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
-# the reference definitions. This must be a list of .bib files. The .bib
-# extension is automatically appended if omitted. This requires the bibtex tool
-# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
-# For LaTeX the style of the bibliography can be controlled using
-# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
-# search path. See also \cite for info how to create references.
-
-CITE_BIB_FILES         =
-
-#---------------------------------------------------------------------------
-# Configuration options related to warning and progress messages
-#---------------------------------------------------------------------------
-
-# The QUIET tag can be used to turn on/off the messages that are generated to
-# standard output by doxygen. If QUIET is set to YES this implies that the
-# messages are off.
-# The default value is: NO.
-
-QUIET                  = NO
-
-# The WARNINGS tag can be used to turn on/off the warning messages that are
-# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
-# this implies that the warnings are on.
-#
-# Tip: Turn warnings on while writing the documentation.
-# The default value is: YES.
-
-WARNINGS               = YES
-
-# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
-# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
-# will automatically be disabled.
-# The default value is: YES.
-
-WARN_IF_UNDOCUMENTED   = YES
-
-# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
-# potential errors in the documentation, such as not documenting some parameters
-# in a documented function, or documenting parameters that don't exist or using
-# markup commands wrongly.
-# The default value is: YES.
-
-WARN_IF_DOC_ERROR      = YES
-
-# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
-# are documented, but have no documentation for their parameters or return
-# value. If set to NO, doxygen will only warn about wrong or incomplete
-# parameter documentation, but not about the absence of documentation.
-# The default value is: NO.
-
-WARN_NO_PARAMDOC       = NO
-
-# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
-# a warning is encountered.
-# The default value is: NO.
-
-WARN_AS_ERROR          = NO
-
-# The WARN_FORMAT tag determines the format of the warning messages that doxygen
-# can produce. The string should contain the $file, $line, and $text tags, which
-# will be replaced by the file and line number from which the warning originated
-# and the warning text. Optionally the format may contain $version, which will
-# be replaced by the version of the file (if it could be obtained via
-# FILE_VERSION_FILTER)
-# The default value is: $file:$line: $text.
-
-WARN_FORMAT            = "$file:$line: $text"
-
-# The WARN_LOGFILE tag can be used to specify a file to which warning and error
-# messages should be written. If left blank the output is written to standard
-# error (stderr).
-
-WARN_LOGFILE           =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the input files
-#---------------------------------------------------------------------------
-
-# The INPUT tag is used to specify the files and/or directories that contain
-# documented source files. You may enter file names like myfile.cpp or
-# directories like /usr/src/myproject. Separate the files or directories with
-# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
-# Note: If this tag is empty the current directory is searched.
-
-INPUT                  = include src submodules README.md
-
-# This tag can be used to specify the character encoding of the source files
-# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
-# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
-# documentation (see: http://www.gnu.org/software/libiconv) for the list of
-# possible encodings.
-# The default value is: UTF-8.
-
-INPUT_ENCODING         = UTF-8
-
-# If the value of the INPUT tag contains directories, you can use the
-# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
-# *.h) to filter out the source-files in the directories.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# read by doxygen.
-#
-# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
-# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
-# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
-# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl,
-# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js.
-
-FILE_PATTERNS          =
-
-# The RECURSIVE tag can be used to specify whether or not subdirectories should
-# be searched for input files as well.
-# The default value is: NO.
-
-RECURSIVE              = YES
-
-# The EXCLUDE tag can be used to specify files and/or directories that should be
-# excluded from the INPUT source files. This way you can easily exclude a
-# subdirectory from a directory tree whose root is specified with the INPUT tag.
-#
-# Note that relative paths are relative to the directory from which doxygen is
-# run.
-
-EXCLUDE                =
-
-# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
-# directories that are symbolic links (a Unix file system feature) are excluded
-# from the input.
-# The default value is: NO.
-
-EXCLUDE_SYMLINKS       = NO
-
-# If the value of the INPUT tag contains directories, you can use the
-# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
-# certain files from those directories.
-#
-# Note that the wildcards are matched against the file with absolute path, so to
-# exclude all test directories for example use the pattern */test/*
-
-EXCLUDE_PATTERNS       =
-
-# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
-# (namespaces, classes, functions, etc.) that should be excluded from the
-# output. The symbol name can be a fully qualified name, a word, or if the
-# wildcard * is used, a substring. Examples: ANamespace, AClass,
-# AClass::ANamespace, ANamespace::*Test
-#
-# Note that the wildcards are matched against the file with absolute path, so to
-# exclude all test directories use the pattern */test/*
-
-EXCLUDE_SYMBOLS        =
-
-# The EXAMPLE_PATH tag can be used to specify one or more files or directories
-# that contain example code fragments that are included (see the \include
-# command).
-
-EXAMPLE_PATH           =
-
-# If the value of the EXAMPLE_PATH tag contains directories, you can use the
-# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
-# *.h) to filter out the source-files in the directories. If left blank all
-# files are included.
-
-EXAMPLE_PATTERNS       =
-
-# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
-# searched for input files to be used with the \include or \dontinclude commands
-# irrespective of the value of the RECURSIVE tag.
-# The default value is: NO.
-
-EXAMPLE_RECURSIVE      = NO
-
-# The IMAGE_PATH tag can be used to specify one or more files or directories
-# that contain images that are to be included in the documentation (see the
-# \image command).
-
-IMAGE_PATH             =
-
-# The INPUT_FILTER tag can be used to specify a program that doxygen should
-# invoke to filter for each input file. Doxygen will invoke the filter program
-# by executing (via popen()) the command:
-#
-# <filter> <input-file>
-#
-# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
-# name of an input file. Doxygen will then use the output that the filter
-# program writes to standard output. If FILTER_PATTERNS is specified, this tag
-# will be ignored.
-#
-# Note that the filter must not add or remove lines; it is applied before the
-# code is scanned, but not when the output code is generated. If lines are added
-# or removed, the anchors will not be placed correctly.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# properly processed by doxygen.
-
-INPUT_FILTER           =
-
-# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
-# basis. Doxygen will compare the file name with each pattern and apply the
-# filter if there is a match. The filters are a list of the form: pattern=filter
-# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
-# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
-# patterns match the file name, INPUT_FILTER is applied.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# properly processed by doxygen.
-
-FILTER_PATTERNS        =
-
-# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
-# INPUT_FILTER) will also be used to filter the input files that are used for
-# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
-# The default value is: NO.
-
-FILTER_SOURCE_FILES    = NO
-
-# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
-# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
-# it is also possible to disable source filtering for a specific pattern using
-# *.ext= (so without naming a filter).
-# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
-
-FILTER_SOURCE_PATTERNS =
-
-# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
-# is part of the input, its contents will be placed on the main page
-# (index.html). This can be useful if you have a project on for instance GitHub
-# and want to reuse the introduction page also for the doxygen output.
-
-USE_MDFILE_AS_MAINPAGE = README.md
-
-#---------------------------------------------------------------------------
-# Configuration options related to source browsing
-#---------------------------------------------------------------------------
-
-# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
-# generated. Documented entities will be cross-referenced with these sources.
-#
-# Note: To get rid of all source code in the generated output, make sure that
-# also VERBATIM_HEADERS is set to NO.
-# The default value is: NO.
-
-SOURCE_BROWSER         = NO
-
-# Setting the INLINE_SOURCES tag to YES will include the body of functions,
-# classes and enums directly into the documentation.
-# The default value is: NO.
-
-INLINE_SOURCES         = NO
-
-# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
-# special comment blocks from generated source code fragments. Normal C, C++ and
-# Fortran comments will always remain visible.
-# The default value is: YES.
-
-STRIP_CODE_COMMENTS    = YES
-
-# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
-# function all documented functions referencing it will be listed.
-# The default value is: NO.
-
-REFERENCED_BY_RELATION = NO
-
-# If the REFERENCES_RELATION tag is set to YES then for each documented function
-# all documented entities called/used by that function will be listed.
-# The default value is: NO.
-
-REFERENCES_RELATION    = NO
-
-# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
-# to YES then the hyperlinks from functions in REFERENCES_RELATION and
-# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
-# link to the documentation.
-# The default value is: YES.
-
-REFERENCES_LINK_SOURCE = YES
-
-# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
-# source code will show a tooltip with additional information such as prototype,
-# brief description and links to the definition and documentation. Since this
-# will make the HTML file larger and loading of large files a bit slower, you
-# can opt to disable this feature.
-# The default value is: YES.
-# This tag requires that the tag SOURCE_BROWSER is set to YES.
-
-SOURCE_TOOLTIPS        = YES
-
-# If the USE_HTAGS tag is set to YES then the references to source code will
-# point to the HTML generated by the htags(1) tool instead of doxygen built-in
-# source browser. The htags tool is part of GNU's global source tagging system
-# (see http://www.gnu.org/software/global/global.html). You will need version
-# 4.8.6 or higher.
-#
-# To use it do the following:
-# - Install the latest version of global
-# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
-# - Make sure the INPUT points to the root of the source tree
-# - Run doxygen as normal
-#
-# Doxygen will invoke htags (and that will in turn invoke gtags), so these
-# tools must be available from the command line (i.e. in the search path).
-#
-# The result: instead of the source browser generated by doxygen, the links to
-# source code will now point to the output of htags.
-# The default value is: NO.
-# This tag requires that the tag SOURCE_BROWSER is set to YES.
-
-USE_HTAGS              = NO
-
-# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
-# verbatim copy of the header file for each class for which an include is
-# specified. Set to NO to disable this.
-# See also: Section \class.
-# The default value is: YES.
-
-VERBATIM_HEADERS       = YES
-
-# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
-# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
-# cost of reduced performance. This can be particularly helpful with template
-# rich C++ code for which doxygen's built-in parser lacks the necessary type
-# information.
-# Note: The availability of this option depends on whether or not doxygen was
-# generated with the -Duse-libclang=ON option for CMake.
-# The default value is: NO.
-
-CLANG_ASSISTED_PARSING = NO
-
-# If clang assisted parsing is enabled you can provide the compiler with command
-# line options that you would normally use when invoking the compiler. Note that
-# the include paths will already be set by doxygen for the files and directories
-# specified with INPUT and INCLUDE_PATH.
-# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
-
-CLANG_OPTIONS          =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the alphabetical class index
-#---------------------------------------------------------------------------
-
-# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
-# compounds will be generated. Enable this if the project contains a lot of
-# classes, structs, unions or interfaces.
-# The default value is: YES.
-
-ALPHABETICAL_INDEX     = YES
-
-# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
-# which the alphabetical index list will be split.
-# Minimum value: 1, maximum value: 20, default value: 5.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-COLS_IN_ALPHA_INDEX    = 5
-
-# In case all classes in a project start with a common prefix, all classes will
-# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
-# can be used to specify a prefix (or a list of prefixes) that should be ignored
-# while generating the index headers.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-IGNORE_PREFIX          =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the HTML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
-# The default value is: YES.
-
-GENERATE_HTML          = YES
-
-# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: html.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_OUTPUT            = html
-
-# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
-# generated HTML page (for example: .htm, .php, .asp).
-# The default value is: .html.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_FILE_EXTENSION    = .html
-
-# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
-# each generated HTML page. If the tag is left blank doxygen will generate a
-# standard header.
-#
-# To get valid HTML the header file that includes any scripts and style sheets
-# that doxygen needs, which is dependent on the configuration options used (e.g.
-# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
-# default header using
-# doxygen -w html new_header.html new_footer.html new_stylesheet.css
-# YourConfigFile
-# and then modify the file new_header.html. See also section "Doxygen usage"
-# for information on how to generate the default header that doxygen normally
-# uses.
-# Note: The header is subject to change so you typically have to regenerate the
-# default header when upgrading to a newer version of doxygen. For a description
-# of the possible markers and block names see the documentation.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_HEADER            =
-
-# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
-# generated HTML page. If the tag is left blank doxygen will generate a standard
-# footer. See HTML_HEADER for more information on how to generate a default
-# footer and what special commands can be used inside the footer. See also
-# section "Doxygen usage" for information on how to generate the default footer
-# that doxygen normally uses.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_FOOTER            =
-
-# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
-# sheet that is used by each HTML page. It can be used to fine-tune the look of
-# the HTML output. If left blank doxygen will generate a default style sheet.
-# See also section "Doxygen usage" for information on how to generate the style
-# sheet that doxygen normally uses.
-# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
-# it is more robust and this tag (HTML_STYLESHEET) will in the future become
-# obsolete.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_STYLESHEET        =
-
-# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
-# cascading style sheets that are included after the standard style sheets
-# created by doxygen. Using this option one can overrule certain style aspects.
-# This is preferred over using HTML_STYLESHEET since it does not replace the
-# standard style sheet and is therefore more robust against future updates.
-# Doxygen will copy the style sheet files to the output directory.
-# Note: The order of the extra style sheet files is of importance (e.g. the last
-# style sheet in the list overrules the setting of the previous ones in the
-# list). For an example see the documentation.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_EXTRA_STYLESHEET  =
-
-# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
-# other source files which should be copied to the HTML output directory. Note
-# that these files will be copied to the base HTML output directory. Use the
-# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
-# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
-# files will be copied as-is; there are no commands or markers available.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_EXTRA_FILES       =
-
-# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
-# will adjust the colors in the style sheet and background images according to
-# this color. Hue is specified as an angle on a colorwheel, see
-# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
-# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
-# purple, and 360 is red again.
-# Minimum value: 0, maximum value: 359, default value: 220.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_HUE    = 220
-
-# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
-# in the HTML output. For a value of 0 the output will use grayscales only. A
-# value of 255 will produce the most vivid colors.
-# Minimum value: 0, maximum value: 255, default value: 100.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_SAT    = 100
-
-# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
-# luminance component of the colors in the HTML output. Values below 100
-# gradually make the output lighter, whereas values above 100 make the output
-# darker. The value divided by 100 is the actual gamma applied, so 80 represents
-# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
-# change the gamma.
-# Minimum value: 40, maximum value: 240, default value: 80.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_GAMMA  = 80
-
-# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
-# page will contain the date and time when the page was generated. Setting this
-# to YES can help to show when doxygen was last run and thus if the
-# documentation is up to date.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_TIMESTAMP         = NO
-
-# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
-# documentation will contain sections that can be hidden and shown after the
-# page has loaded.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_DYNAMIC_SECTIONS  = NO
-
-# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
-# shown in the various tree structured indices initially; the user can expand
-# and collapse entries dynamically later on. Doxygen will expand the tree to
-# such a level that at most the specified number of entries are visible (unless
-# a fully collapsed tree already exceeds this amount). So setting the number of
-# entries 1 will produce a full collapsed tree by default. 0 is a special value
-# representing an infinite number of entries and will result in a full expanded
-# tree by default.
-# Minimum value: 0, maximum value: 9999, default value: 100.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_INDEX_NUM_ENTRIES = 100
-
-# If the GENERATE_DOCSET tag is set to YES, additional index files will be
-# generated that can be used as input for Apple's Xcode 3 integrated development
-# environment (see: http://developer.apple.com/tools/xcode/), introduced with
-# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
-# Makefile in the HTML output directory. Running make will produce the docset in
-# that directory and running make install will install the docset in
-# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
-# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
-# for more information.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_DOCSET        = NO
-
-# This tag determines the name of the docset feed. A documentation feed provides
-# an umbrella under which multiple documentation sets from a single provider
-# (such as a company or product suite) can be grouped.
-# The default value is: Doxygen generated docs.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_FEEDNAME        = "Doxygen generated docs"
-
-# This tag specifies a string that should uniquely identify the documentation
-# set bundle. This should be a reverse domain-name style string, e.g.
-# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_BUNDLE_ID       = org.doxygen.Project
-
-# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
-# the documentation publisher. This should be a reverse domain-name style
-# string, e.g. com.mycompany.MyDocSet.documentation.
-# The default value is: org.doxygen.Publisher.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
-
-# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
-# The default value is: Publisher.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_PUBLISHER_NAME  = Publisher
-
-# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
-# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
-# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
-# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
-# Windows.
-#
-# The HTML Help Workshop contains a compiler that can convert all HTML output
-# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
-# files are now used as the Windows 98 help format, and will replace the old
-# Windows help format (.hlp) on all Windows platforms in the future. Compressed
-# HTML files also contain an index, a table of contents, and you can search for
-# words in the documentation. The HTML workshop also contains a viewer for
-# compressed HTML files.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_HTMLHELP      = NO
-
-# The CHM_FILE tag can be used to specify the file name of the resulting .chm
-# file. You can add a path in front of the file if the result should not be
-# written to the html output directory.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-CHM_FILE               =
-
-# The HHC_LOCATION tag can be used to specify the location (absolute path
-# including file name) of the HTML help compiler (hhc.exe). If non-empty,
-# doxygen will try to run the HTML help compiler on the generated index.hhp.
-# The file has to be specified with full path.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-HHC_LOCATION           =
-
-# The GENERATE_CHI flag controls if a separate .chi index file is generated
-# (YES) or that it should be included in the master .chm file (NO).
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-GENERATE_CHI           = NO
-
-# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
-# and project file content.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-CHM_INDEX_ENCODING     =
-
-# The BINARY_TOC flag controls whether a binary table of contents is generated
-# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
-# enables the Previous and Next buttons.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-BINARY_TOC             = NO
-
-# The TOC_EXPAND flag can be set to YES to add extra items for group members to
-# the table of contents of the HTML help documentation and to the tree view.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-TOC_EXPAND             = NO
-
-# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
-# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
-# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
-# (.qch) of the generated HTML documentation.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_QHP           = NO
-
-# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
-# the file name of the resulting .qch file. The path specified is relative to
-# the HTML output folder.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QCH_FILE               =
-
-# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
-# Project output. For more information please see Qt Help Project / Namespace
-# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_NAMESPACE          = org.doxygen.Project
-
-# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
-# Help Project output. For more information please see Qt Help Project / Virtual
-# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
-# folders).
-# The default value is: doc.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_VIRTUAL_FOLDER     = doc
-
-# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
-# filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
-# filters).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_CUST_FILTER_NAME   =
-
-# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
-# custom filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
-# filters).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_CUST_FILTER_ATTRS  =
-
-# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
-# project's filter section matches. Qt Help Project / Filter Attributes (see:
-# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_SECT_FILTER_ATTRS  =
-
-# The QHG_LOCATION tag can be used to specify the location of Qt's
-# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
-# generated .qhp file.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHG_LOCATION           =
-
-# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
-# generated, together with the HTML files, they form an Eclipse help plugin. To
-# install this plugin and make it available under the help contents menu in
-# Eclipse, the contents of the directory containing the HTML and XML files needs
-# to be copied into the plugins directory of eclipse. The name of the directory
-# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
-# After copying Eclipse needs to be restarted before the help appears.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_ECLIPSEHELP   = NO
-
-# A unique identifier for the Eclipse help plugin. When installing the plugin
-# the directory name containing the HTML and XML files should also have this
-# name. Each documentation set should have its own identifier.
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
-
-ECLIPSE_DOC_ID         = org.doxygen.Project
-
-# If you want full control over the layout of the generated HTML pages it might
-# be necessary to disable the index and replace it with your own. The
-# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
-# of each HTML page. A value of NO enables the index and the value YES disables
-# it. Since the tabs in the index contain the same information as the navigation
-# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-DISABLE_INDEX          = NO
-
-# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
-# structure should be generated to display hierarchical information. If the tag
-# value is set to YES, a side panel will be generated containing a tree-like
-# index structure (just like the one that is generated for HTML Help). For this
-# to work a browser that supports JavaScript, DHTML, CSS and frames is required
-# (i.e. any modern browser). Windows users are probably better off using the
-# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
-# further fine-tune the look of the index. As an example, the default style
-# sheet generated by doxygen has an example that shows how to put an image at
-# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
-# the same information as the tab index, you could consider setting
-# DISABLE_INDEX to YES when enabling this option.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_TREEVIEW      = NO
-
-# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
-# doxygen will group on one line in the generated HTML documentation.
-#
-# Note that a value of 0 will completely suppress the enum values from appearing
-# in the overview section.
-# Minimum value: 0, maximum value: 20, default value: 4.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-ENUM_VALUES_PER_LINE   = 4
-
-# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
-# to set the initial width (in pixels) of the frame in which the tree is shown.
-# Minimum value: 0, maximum value: 1500, default value: 250.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-TREEVIEW_WIDTH         = 250
-
-# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
-# external symbols imported via tag files in a separate window.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-EXT_LINKS_IN_WINDOW    = NO
-
-# Use this tag to change the font size of LaTeX formulas included as images in
-# the HTML documentation. When you change the font size after a successful
-# doxygen run you need to manually remove any form_*.png images from the HTML
-# output directory to force them to be regenerated.
-# Minimum value: 8, maximum value: 50, default value: 10.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-FORMULA_FONTSIZE       = 10
-
-# Use the FORMULA_TRANPARENT tag to determine whether or not the images
-# generated for formulas are transparent PNGs. Transparent PNGs are not
-# supported properly for IE 6.0, but are supported on all modern browsers.
-#
-# Note that when changing this option you need to delete any form_*.png files in
-# the HTML output directory before the changes have effect.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-FORMULA_TRANSPARENT    = YES
-
-# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
-# http://www.mathjax.org) which uses client side Javascript for the rendering
-# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
-# installed or if you want to formulas look prettier in the HTML output. When
-# enabled you may also need to install MathJax separately and configure the path
-# to it using the MATHJAX_RELPATH option.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-USE_MATHJAX            = NO
-
-# When MathJax is enabled you can set the default output format to be used for
-# the MathJax output. See the MathJax site (see:
-# http://docs.mathjax.org/en/latest/output.html) for more details.
-# Possible values are: HTML-CSS (which is slower, but has the best
-# compatibility), NativeMML (i.e. MathML) and SVG.
-# The default value is: HTML-CSS.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_FORMAT         = HTML-CSS
-
-# When MathJax is enabled you need to specify the location relative to the HTML
-# output directory using the MATHJAX_RELPATH option. The destination directory
-# should contain the MathJax.js script. For instance, if the mathjax directory
-# is located at the same level as the HTML output directory, then
-# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
-# Content Delivery Network so you can quickly see the result without installing
-# MathJax. However, it is strongly recommended to install a local copy of
-# MathJax from http://www.mathjax.org before deployment.
-# The default value is: http://cdn.mathjax.org/mathjax/latest.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
-
-# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
-# extension names that should be enabled during MathJax rendering. For example
-# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_EXTENSIONS     =
-
-# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
-# of code that will be used on startup of the MathJax code. See the MathJax site
-# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
-# example see the documentation.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_CODEFILE       =
-
-# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
-# the HTML output. The underlying search engine uses javascript and DHTML and
-# should work on any modern browser. Note that when using HTML help
-# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
-# there is already a search function so this one should typically be disabled.
-# For large projects the javascript based search engine can be slow, then
-# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
-# search using the keyboard; to jump to the search box use <access key> + S
-# (what the <access key> is depends on the OS and browser, but it is typically
-# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
-# key> to jump into the search results window, the results can be navigated
-# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
-# the search. The filter options can be selected when the cursor is inside the
-# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
-# to select a filter and <Enter> or <escape> to activate or cancel the filter
-# option.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-SEARCHENGINE           = YES
-
-# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
-# implemented using a web server instead of a web client using Javascript. There
-# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
-# setting. When disabled, doxygen will generate a PHP script for searching and
-# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
-# and searching needs to be provided by external tools. See the section
-# "External Indexing and Searching" for details.
-# The default value is: NO.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SERVER_BASED_SEARCH    = NO
-
-# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
-# script for searching. Instead the search results are written to an XML file
-# which needs to be processed by an external indexer. Doxygen will invoke an
-# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
-# search results.
-#
-# Doxygen ships with an example indexer (doxyindexer) and search engine
-# (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: http://xapian.org/).
-#
-# See the section "External Indexing and Searching" for details.
-# The default value is: NO.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTERNAL_SEARCH        = NO
-
-# The SEARCHENGINE_URL should point to a search engine hosted by a web server
-# which will return the search results when EXTERNAL_SEARCH is enabled.
-#
-# Doxygen ships with an example indexer (doxyindexer) and search engine
-# (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: http://xapian.org/). See the section "External Indexing and
-# Searching" for details.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SEARCHENGINE_URL       =
-
-# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
-# search data is written to a file for indexing by an external tool. With the
-# SEARCHDATA_FILE tag the name of this file can be specified.
-# The default file is: searchdata.xml.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SEARCHDATA_FILE        = searchdata.xml
-
-# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
-# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
-# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
-# projects and redirect the results back to the right project.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTERNAL_SEARCH_ID     =
-
-# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
-# projects other than the one defined by this configuration file, but that are
-# all added to the same external search index. Each project needs to have a
-# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
-# to a relative location where the documentation can be found. The format is:
-# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTRA_SEARCH_MAPPINGS  =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the LaTeX output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
-# The default value is: YES.
-
-GENERATE_LATEX         = YES
-
-# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: latex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_OUTPUT           = latex
-
-# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
-# invoked.
-#
-# Note that when enabling USE_PDFLATEX this option is only used for generating
-# bitmaps for formulas in the HTML output, but not in the Makefile that is
-# written to the output directory.
-# The default file is: latex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_CMD_NAME         = latex
-
-# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
-# index for LaTeX.
-# The default file is: makeindex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-MAKEINDEX_CMD_NAME     = makeindex
-
-# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
-# documents. This may be useful for small projects and may help to save some
-# trees in general.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-COMPACT_LATEX          = NO
-
-# The PAPER_TYPE tag can be used to set the paper type that is used by the
-# printer.
-# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
-# 14 inches) and executive (7.25 x 10.5 inches).
-# The default value is: a4.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-PAPER_TYPE             = a4
-
-# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
-# that should be included in the LaTeX output. The package can be specified just
-# by its name or with the correct syntax as to be used with the LaTeX
-# \usepackage command. To get the times font for instance you can specify :
-# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
-# To use the option intlimits with the amsmath package you can specify:
-# EXTRA_PACKAGES=[intlimits]{amsmath}
-# If left blank no extra packages will be included.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-EXTRA_PACKAGES         =
-
-# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
-# generated LaTeX document. The header should contain everything until the first
-# chapter. If it is left blank doxygen will generate a standard header. See
-# section "Doxygen usage" for information on how to let doxygen write the
-# default header to a separate file.
-#
-# Note: Only use a user-defined header if you know what you are doing! The
-# following commands have a special meaning inside the header: $title,
-# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
-# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
-# string, for the replacement values of the other commands the user is referred
-# to HTML_HEADER.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_HEADER           =
-
-# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
-# generated LaTeX document. The footer should contain everything after the last
-# chapter. If it is left blank doxygen will generate a standard footer. See
-# LATEX_HEADER for more information on how to generate a default footer and what
-# special commands can be used inside the footer.
-#
-# Note: Only use a user-defined footer if you know what you are doing!
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_FOOTER           =
-
-# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
-# LaTeX style sheets that are included after the standard style sheets created
-# by doxygen. Using this option one can overrule certain style aspects. Doxygen
-# will copy the style sheet files to the output directory.
-# Note: The order of the extra style sheet files is of importance (e.g. the last
-# style sheet in the list overrules the setting of the previous ones in the
-# list).
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_EXTRA_STYLESHEET =
-
-# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
-# other source files which should be copied to the LATEX_OUTPUT output
-# directory. Note that the files will be copied as-is; there are no commands or
-# markers available.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_EXTRA_FILES      =
-
-# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
-# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
-# contain links (just like the HTML output) instead of page references. This
-# makes the output suitable for online browsing using a PDF viewer.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-PDF_HYPERLINKS         = YES
-
-# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
-# the PDF file directly from the LaTeX files. Set this option to YES, to get a
-# higher quality PDF documentation.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-USE_PDFLATEX           = YES
-
-# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
-# command to the generated LaTeX files. This will instruct LaTeX to keep running
-# if errors occur, instead of asking the user for help. This option is also used
-# when generating formulas in HTML.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_BATCHMODE        = NO
-
-# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
-# index chapters (such as File Index, Compound Index, etc.) in the output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_HIDE_INDICES     = NO
-
-# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
-# code with syntax highlighting in the LaTeX output.
-#
-# Note that which sources are shown also depends on other settings such as
-# SOURCE_BROWSER.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_SOURCE_CODE      = NO
-
-# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
-# bibliography, e.g. plainnat, or ieeetr. See
-# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
-# The default value is: plain.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_BIB_STYLE        = plain
-
-# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
-# page will contain the date and time when the page was generated. Setting this
-# to NO can help when comparing the output of multiple runs.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_TIMESTAMP        = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the RTF output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
-# RTF output is optimized for Word 97 and may not look too pretty with other RTF
-# readers/editors.
-# The default value is: NO.
-
-GENERATE_RTF           = NO
-
-# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: rtf.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_OUTPUT             = rtf
-
-# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
-# documents. This may be useful for small projects and may help to save some
-# trees in general.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-COMPACT_RTF            = NO
-
-# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
-# contain hyperlink fields. The RTF file will contain links (just like the HTML
-# output) instead of page references. This makes the output suitable for online
-# browsing using Word or some other Word compatible readers that support those
-# fields.
-#
-# Note: WordPad (write) and others do not support links.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_HYPERLINKS         = NO
-
-# Load stylesheet definitions from file. Syntax is similar to doxygen's config
-# file, i.e. a series of assignments. You only have to provide replacements,
-# missing definitions are set to their default value.
-#
-# See also section "Doxygen usage" for information on how to generate the
-# default style sheet that doxygen normally uses.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_STYLESHEET_FILE    =
-
-# Set optional variables used in the generation of an RTF document. Syntax is
-# similar to doxygen's config file. A template extensions file can be generated
-# using doxygen -e rtf extensionFile.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_EXTENSIONS_FILE    =
-
-# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
-# with syntax highlighting in the RTF output.
-#
-# Note that which sources are shown also depends on other settings such as
-# SOURCE_BROWSER.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_SOURCE_CODE        = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the man page output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
-# classes and files.
-# The default value is: NO.
-
-GENERATE_MAN           = NO
-
-# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it. A directory man3 will be created inside the directory specified by
-# MAN_OUTPUT.
-# The default directory is: man.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_OUTPUT             = man
-
-# The MAN_EXTENSION tag determines the extension that is added to the generated
-# man pages. In case the manual section does not start with a number, the number
-# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
-# optional.
-# The default value is: .3.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_EXTENSION          = .3
-
-# The MAN_SUBDIR tag determines the name of the directory created within
-# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
-# MAN_EXTENSION with the initial . removed.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_SUBDIR             =
-
-# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
-# will generate one additional man file for each entity documented in the real
-# man page(s). These additional files only source the real man page, but without
-# them the man command would be unable to find the correct page.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_LINKS              = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the XML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
-# captures the structure of the code including all documentation.
-# The default value is: NO.
-
-GENERATE_XML           = NO
-
-# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: xml.
-# This tag requires that the tag GENERATE_XML is set to YES.
-
-XML_OUTPUT             = xml
-
-# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
-# listings (including syntax highlighting and cross-referencing information) to
-# the XML output. Note that enabling this will significantly increase the size
-# of the XML output.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_XML is set to YES.
-
-XML_PROGRAMLISTING     = YES
-
-#---------------------------------------------------------------------------
-# Configuration options related to the DOCBOOK output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
-# that can be used to generate PDF.
-# The default value is: NO.
-
-GENERATE_DOCBOOK       = NO
-
-# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
-# front of it.
-# The default directory is: docbook.
-# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
-
-DOCBOOK_OUTPUT         = docbook
-
-# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
-# program listings (including syntax highlighting and cross-referencing
-# information) to the DOCBOOK output. Note that enabling this will significantly
-# increase the size of the DOCBOOK output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
-
-DOCBOOK_PROGRAMLISTING = NO
-
-#---------------------------------------------------------------------------
-# Configuration options for the AutoGen Definitions output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
-# AutoGen Definitions (see http://autogen.sf.net) file that captures the
-# structure of the code including all documentation. Note that this feature is
-# still experimental and incomplete at the moment.
-# The default value is: NO.
-
-GENERATE_AUTOGEN_DEF   = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the Perl module output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
-# file that captures the structure of the code including all documentation.
-#
-# Note that this feature is still experimental and incomplete at the moment.
-# The default value is: NO.
-
-GENERATE_PERLMOD       = NO
-
-# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
-# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
-# output from the Perl module output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_LATEX          = NO
-
-# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
-# formatted so it can be parsed by a human reader. This is useful if you want to
-# understand what is going on. On the other hand, if this tag is set to NO, the
-# size of the Perl module output will be much smaller and Perl will parse it
-# just the same.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_PRETTY         = YES
-
-# The names of the make variables in the generated doxyrules.make file are
-# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
-# so different doxyrules.make files included by the same Makefile don't
-# overwrite each other's variables.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_MAKEVAR_PREFIX =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the preprocessor
-#---------------------------------------------------------------------------
-
-# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
-# C-preprocessor directives found in the sources and include files.
-# The default value is: YES.
-
-ENABLE_PREPROCESSING   = YES
-
-# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
-# in the source code. If set to NO, only conditional compilation will be
-# performed. Macro expansion can be done in a controlled way by setting
-# EXPAND_ONLY_PREDEF to YES.
-# The default value is: NO.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-MACRO_EXPANSION        = NO
-
-# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
-# the macro expansion is limited to the macros specified with the PREDEFINED and
-# EXPAND_AS_DEFINED tags.
-# The default value is: NO.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-EXPAND_ONLY_PREDEF     = NO
-
-# If the SEARCH_INCLUDES tag is set to YES, the include files in the
-# INCLUDE_PATH will be searched if a #include is found.
-# The default value is: YES.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-SEARCH_INCLUDES        = YES
-
-# The INCLUDE_PATH tag can be used to specify one or more directories that
-# contain include files that are not input files but should be processed by the
-# preprocessor.
-# This tag requires that the tag SEARCH_INCLUDES is set to YES.
-
-INCLUDE_PATH           =
-
-# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
-# patterns (like *.h and *.hpp) to filter out the header-files in the
-# directories. If left blank, the patterns specified with FILE_PATTERNS will be
-# used.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-INCLUDE_FILE_PATTERNS  =
-
-# The PREDEFINED tag can be used to specify one or more macro names that are
-# defined before the preprocessor is started (similar to the -D option of e.g.
-# gcc). The argument of the tag is a list of macros of the form: name or
-# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
-# is assumed. To prevent a macro definition from being undefined via #undef or
-# recursively expanded use the := operator instead of the = operator.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-PREDEFINED             =
-
-# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
-# tag can be used to specify a list of macro names that should be expanded. The
-# macro definition that is found in the sources will be used. Use the PREDEFINED
-# tag if you want to use a different macro definition that overrules the
-# definition found in the source code.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-EXPAND_AS_DEFINED      =
-
-# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
-# remove all references to function-like macros that are alone on a line, have
-# an all uppercase name, and do not end with a semicolon. Such function macros
-# are typically used for boiler-plate code, and will confuse the parser if not
-# removed.
-# The default value is: YES.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-SKIP_FUNCTION_MACROS   = YES
-
-#---------------------------------------------------------------------------
-# Configuration options related to external references
-#---------------------------------------------------------------------------
-
-# The TAGFILES tag can be used to specify one or more tag files. For each tag
-# file the location of the external documentation should be added. The format of
-# a tag file without this location is as follows:
-# TAGFILES = file1 file2 ...
-# Adding location for the tag files is done as follows:
-# TAGFILES = file1=loc1 "file2 = loc2" ...
-# where loc1 and loc2 can be relative or absolute paths or URLs. See the
-# section "Linking to external documentation" for more information about the use
-# of tag files.
-# Note: Each tag file must have a unique name (where the name does NOT include
-# the path). If a tag file is not located in the directory in which doxygen is
-# run, you must also specify the path to the tagfile here.
-
-TAGFILES               =
-
-# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
-# tag file that is based on the input files it reads. See section "Linking to
-# external documentation" for more information about the usage of tag files.
-
-GENERATE_TAGFILE       =
-
-# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
-# the class index. If set to NO, only the inherited external classes will be
-# listed.
-# The default value is: NO.
-
-ALLEXTERNALS           = NO
-
-# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
-# in the modules index. If set to NO, only the current project's groups will be
-# listed.
-# The default value is: YES.
-
-EXTERNAL_GROUPS        = YES
-
-# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
-# the related pages index. If set to NO, only the current project's pages will
-# be listed.
-# The default value is: YES.
-
-EXTERNAL_PAGES         = YES
-
-# The PERL_PATH should be the absolute path and name of the perl script
-# interpreter (i.e. the result of 'which perl').
-# The default file (with absolute path) is: /usr/bin/perl.
-
-PERL_PATH              = /usr/bin/perl
-
-#---------------------------------------------------------------------------
-# Configuration options related to the dot tool
-#---------------------------------------------------------------------------
-
-# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
-# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
-# NO turns the diagrams off. Note that this option also works with HAVE_DOT
-# disabled, but it is recommended to install and use dot, since it yields more
-# powerful graphs.
-# The default value is: YES.
-
-CLASS_DIAGRAMS         = YES
-
-# You can define message sequence charts within doxygen comments using the \msc
-# command. Doxygen will then run the mscgen tool (see:
-# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
-# documentation. The MSCGEN_PATH tag allows you to specify the directory where
-# the mscgen tool resides. If left empty the tool is assumed to be found in the
-# default search path.
-
-MSCGEN_PATH            =
-
-# You can include diagrams made with dia in doxygen documentation. Doxygen will
-# then run dia to produce the diagram and insert it in the documentation. The
-# DIA_PATH tag allows you to specify the directory where the dia binary resides.
-# If left empty dia is assumed to be found in the default search path.
-
-DIA_PATH               =
-
-# If set to YES the inheritance and collaboration graphs will hide inheritance
-# and usage relations if the target is undocumented or is not a class.
-# The default value is: YES.
-
-HIDE_UNDOC_RELATIONS   = YES
-
-# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
-# available from the path. This tool is part of Graphviz (see:
-# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
-# Bell Labs. The other options in this section have no effect if this option is
-# set to NO
-# The default value is: YES.
-
-HAVE_DOT               = YES
-
-# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
-# to run in parallel. When set to 0 doxygen will base this on the number of
-# processors available in the system. You can set it explicitly to a value
-# larger than 0 to get control over the balance between CPU load and processing
-# speed.
-# Minimum value: 0, maximum value: 32, default value: 0.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_NUM_THREADS        = 0
-
-# When you want a differently looking font in the dot files that doxygen
-# generates you can specify the font name using DOT_FONTNAME. You need to make
-# sure dot is able to find the font, which can be done by putting it in a
-# standard location or by setting the DOTFONTPATH environment variable or by
-# setting DOT_FONTPATH to the directory containing the font.
-# The default value is: Helvetica.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTNAME           = Helvetica
-
-# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
-# dot graphs.
-# Minimum value: 4, maximum value: 24, default value: 10.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTSIZE           = 10
-
-# By default doxygen will tell dot to use the default font as specified with
-# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
-# the path where dot can find it using this tag.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTPATH           =
-
-# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
-# each documented class showing the direct and indirect inheritance relations.
-# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CLASS_GRAPH            = YES
-
-# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
-# graph for each documented class showing the direct and indirect implementation
-# dependencies (inheritance, containment, and class references variables) of the
-# class with other documented classes.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-COLLABORATION_GRAPH    = YES
-
-# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
-# groups, showing the direct groups dependencies.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GROUP_GRAPHS           = YES
-
-# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
-# collaboration diagrams in a style similar to the OMG's Unified Modeling
-# Language.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-UML_LOOK               = NO
-
-# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
-# class node. If there are many fields or methods and many nodes the graph may
-# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
-# number of items for each type to make the size more manageable. Set this to 0
-# for no limit. Note that the threshold may be exceeded by 50% before the limit
-# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
-# but if the number exceeds 15, the total amount of fields shown is limited to
-# 10.
-# Minimum value: 0, maximum value: 100, default value: 10.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-UML_LIMIT_NUM_FIELDS   = 10
-
-# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
-# collaboration graphs will show the relations between templates and their
-# instances.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-TEMPLATE_RELATIONS     = NO
-
-# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
-# YES then doxygen will generate a graph for each documented file showing the
-# direct and indirect include dependencies of the file with other documented
-# files.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INCLUDE_GRAPH          = YES
-
-# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
-# set to YES then doxygen will generate a graph for each documented file showing
-# the direct and indirect include dependencies of the file with other documented
-# files.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INCLUDED_BY_GRAPH      = YES
-
-# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
-# dependency graph for every global function or class method.
-#
-# Note that enabling this option will significantly increase the time of a run.
-# So in most cases it will be better to enable call graphs for selected
-# functions only using the \callgraph command. Disabling a call graph can be
-# accomplished by means of the command \hidecallgraph.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CALL_GRAPH             = YES
-
-# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
-# dependency graph for every global function or class method.
-#
-# Note that enabling this option will significantly increase the time of a run.
-# So in most cases it will be better to enable caller graphs for selected
-# functions only using the \callergraph command. Disabling a caller graph can be
-# accomplished by means of the command \hidecallergraph.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CALLER_GRAPH           = YES
-
-# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
-# hierarchy of all classes instead of a textual one.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GRAPHICAL_HIERARCHY    = YES
-
-# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
-# dependencies a directory has on other directories in a graphical way. The
-# dependency relations are determined by the #include relations between the
-# files in the directories.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DIRECTORY_GRAPH        = YES
-
-# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
-# generated by dot. For an explanation of the image formats see the section
-# output formats in the documentation of the dot tool (Graphviz (see:
-# http://www.graphviz.org/)).
-# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
-# to make the SVG files visible in IE 9+ (other browsers do not have this
-# requirement).
-# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
-# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
-# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
-# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
-# png:gdiplus:gdiplus.
-# The default value is: png.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_IMAGE_FORMAT       = png
-
-# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
-# enable generation of interactive SVG images that allow zooming and panning.
-#
-# Note that this requires a modern browser other than Internet Explorer. Tested
-# and working are Firefox, Chrome, Safari, and Opera.
-# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
-# the SVG files visible. Older versions of IE do not have SVG support.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INTERACTIVE_SVG        = NO
-
-# The DOT_PATH tag can be used to specify the path where the dot tool can be
-# found. If left blank, it is assumed the dot tool can be found in the path.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_PATH               =
-
-# The DOTFILE_DIRS tag can be used to specify one or more directories that
-# contain dot files that are included in the documentation (see the \dotfile
-# command).
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOTFILE_DIRS           =
-
-# The MSCFILE_DIRS tag can be used to specify one or more directories that
-# contain msc files that are included in the documentation (see the \mscfile
-# command).
-
-MSCFILE_DIRS           =
-
-# The DIAFILE_DIRS tag can be used to specify one or more directories that
-# contain dia files that are included in the documentation (see the \diafile
-# command).
-
-DIAFILE_DIRS           =
-
-# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
-# path where java can find the plantuml.jar file. If left blank, it is assumed
-# PlantUML is not used or called during a preprocessing step. Doxygen will
-# generate a warning when it encounters a \startuml command in this case and
-# will not generate output for the diagram.
-
-PLANTUML_JAR_PATH      =
-
-# When using plantuml, the specified paths are searched for files specified by
-# the !include statement in a plantuml block.
-
-PLANTUML_INCLUDE_PATH  =
-
-# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
-# that will be shown in the graph. If the number of nodes in a graph becomes
-# larger than this value, doxygen will truncate the graph, which is visualized
-# by representing a node as a red box. Note that doxygen if the number of direct
-# children of the root node in a graph is already larger than
-# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
-# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
-# Minimum value: 0, maximum value: 10000, default value: 50.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_GRAPH_MAX_NODES    = 50
-
-# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
-# generated by dot. A depth value of 3 means that only nodes reachable from the
-# root by following a path via at most 3 edges will be shown. Nodes that lay
-# further from the root node will be omitted. Note that setting this option to 1
-# or 2 may greatly reduce the computation time needed for large code bases. Also
-# note that the size of a graph can be further restricted by
-# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
-# Minimum value: 0, maximum value: 1000, default value: 0.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-MAX_DOT_GRAPH_DEPTH    = 0
-
-# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
-# background. This is disabled by default, because dot on Windows does not seem
-# to support this out of the box.
-#
-# Warning: Depending on the platform used, enabling this option may lead to
-# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
-# read).
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_TRANSPARENT        = NO
-
-# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
-# files in one run (i.e. multiple -o and -T options on the command line). This
-# makes dot run faster, but since only newer versions of dot (>1.8.10) support
-# this, this feature is disabled by default.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_MULTI_TARGETS      = NO
-
-# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
-# explaining the meaning of the various boxes and arrows in the dot generated
-# graphs.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GENERATE_LEGEND        = YES
-
-# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
-# files that are used to generate the various graphs.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_CLEANUP            = YES
diff --git a/fixposition_driver_ros1/README.md b/fixposition_driver_ros1/README.md
index 726d789..cdbad39 100644
--- a/fixposition_driver_ros1/README.md
+++ b/fixposition_driver_ros1/README.md
@@ -1,21 +1,5 @@
 # Fixposition ROS1 Driver
 
--   [ROS1 Noetic ![](./../../actions/workflows/build_test_ros.yml/badge.svg)](./../../actions/workflows/build_test_ros.yml)
-
 [ROS](https://www.ros.org/) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product).
 
-## Driver documentation
-
-For installation, usage, and more information, please refer to [Fixposition Docs: ROS Driver](https://docs.fixposition.com/fd/fixposition-ros-driver).
-
-## Fixposition ASCII messages
-
-For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs: I/O messages](https://docs.fixposition.com/fd/i-o-messages).
-
-## Code Documentation
-
-Run `doxygen Doxyfile` to generate Doxygen code documentation.
-
-## License
-
-This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details
\ No newline at end of file
+See the [main README.md](../README.md) for the documentation.
diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp
index 9872d38..a337062 100644
--- a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp
+++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp
@@ -1,150 +1,77 @@
 /**
- *  @file
- *  @brief Convert Data classes to ROS1 msgs
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Convert data to ROS1 msgs
  */
 
-#ifndef __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__
-#define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__
+#ifndef __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__
+#define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__
+
+/* LIBC/STL */
+#include <memory>
+#include <unordered_map>
 
-/* FIXPOSITION DRIVER LIB */
-#include <fixposition_driver_lib/messages/msg_data.hpp>
+/* EXTERNAL */
 #include <fixposition_driver_lib/fixposition_driver.hpp>
+#include <fixposition_driver_lib/helper.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_ros1/ext/ros.hpp>
 
 /* PACKAGE */
-#include <fixposition_driver_ros1/fixposition_driver_node.hpp>
+#include "ros1_msgs.hpp"
 
 namespace fixposition {
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void FpToRosMsg( const OdometryData& data, ros::Publisher& pub);
-void FpToRosMsg(      const ImuData& data, ros::Publisher& pub);
-void FpToRosMsg(       const FP_EOE& data, ros::Publisher& pub);
-void FpToRosMsg(   const FP_GNSSANT& data, ros::Publisher& pub);
-void FpToRosMsg(  const FP_GNSSCORR& data, ros::Publisher& pub);
-void FpToRosMsg(   const FP_IMUBIAS& data, ros::Publisher& pub);
-void FpToRosMsg(       const FP_LLH& data, ros::Publisher& pub);
-void FpToRosMsg(   const FP_ODOMENU& data, ros::Publisher& pub);
-void FpToRosMsg(  const FP_ODOMETRY& data, ros::Publisher& pub);
-void FpToRosMsg(    const FP_ODOMSH& data, ros::Publisher& pub);
-void FpToRosMsg(const FP_ODOMSTATUS& data, ros::Publisher& pub);
-void FpToRosMsg(        const FP_TP& data, ros::Publisher& pub);
-void FpToRosMsg(      const FP_TEXT& data, ros::Publisher& pub);
-
-void FpToRosMsg(const GP_GGA& data, ros::Publisher& pub);
-void FpToRosMsg(const GP_GLL& data, ros::Publisher& pub);
-void FpToRosMsg(const GN_GSA& data, ros::Publisher& pub);
-void FpToRosMsg(const GP_GST& data, ros::Publisher& pub);
-void FpToRosMsg(const GX_GSV& data, ros::Publisher& pub);
-void FpToRosMsg(const GP_HDT& data, ros::Publisher& pub);
-void FpToRosMsg(const GP_RMC& data, ros::Publisher& pub);
-void FpToRosMsg(const GP_VTG& data, ros::Publisher& pub);
-void FpToRosMsg(const GP_ZDA& data, ros::Publisher& pub);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void TfDataToMsg(const TfData& data, geometry_msgs::TransformStamped& msg);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void NavSatFixDataToMsg(const NavSatFixData& data, sensor_msgs::NavSatFix& msg);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void PoseWithCovDataToMsg(const PoseWithCovData& data, geometry_msgs::PoseWithCovariance& msg);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWithCovariance& msg);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pub);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] tf
- */
-void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf);
-
-/**
- * @brief
- *
- * @param[in] tf_map
- * @param[out] static_br_
- * @param[out] br_
- */
-void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void OdomToImuMsg(const FP_ODOMETRY& data, ros::Publisher& pub);
-
-/**
- * @brief
- *
- * @param[in] data
- * @param[out] msg
- */
-void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub);
-
-/**
- * @brief
- *
- * @param[in] stamp
- * @param[in] pos_diff
- * @param[in] prev_cov
- * @param[out] msg
- */
-void JumpWarningMsg(const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, const Eigen::MatrixXd& prev_cov, ros::Publisher& pub);
-
+/* ****************************************************************************************************************** */
+
+void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStamped& msg);
+void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::TransformStamped& msg);
+
+void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub);
+void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload, ros::Publisher& pub);
+void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload,
+                                     ros::Publisher& pub);
+void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub);
+void PublishFpaOdomenuVector3Stamped(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload, ros::Publisher& pub);
+void PublishFpaOdomsh(const fpsdk::common::parser::fpa::FpaOdomshPayload& payload, ros::Publisher& pub);
+void PublishFpaOdomstatus(const fpsdk::common::parser::fpa::FpaOdomstatusPayload& payload, ros::Publisher& pub);
+void PublishFpaLlh(const fpsdk::common::parser::fpa::FpaLlhPayload& payload, ros::Publisher& pub);
+void PublishFpaEoe(const fpsdk::common::parser::fpa::FpaEoePayload& payload, ros::Publisher& pub);
+void PublishFpaImubias(const fpsdk::common::parser::fpa::FpaImubiasPayload& payload, ros::Publisher& pub);
+void PublishFpaGnssant(const fpsdk::common::parser::fpa::FpaGnssantPayload& payload, ros::Publisher& pub);
+void PublishFpaGnsscorr(const fpsdk::common::parser::fpa::FpaGnsscorrPayload& payload, ros::Publisher& pub);
+void PublishFpaTp(const fpsdk::common::parser::fpa::FpaTpPayload& payload, ros::Publisher& pub);
+void PublishFpaText(const fpsdk::common::parser::fpa::FpaTextPayload& payload, ros::Publisher& pub);
+void PublishFpaRawimu(const fpsdk::common::parser::fpa::FpaRawimuPayload& payload, ros::Publisher& pub);
+void PublishFpaCorrimu(const fpsdk::common::parser::fpa::FpaCorrimuPayload& payload, ros::Publisher& pub);
+
+bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* header,
+                            const fpsdk::common::parser::novb::NovbBestgnsspos* payload, ros::Publisher& pub1,
+                            ros::Publisher& pub2);
+
+void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, ros::Publisher& pub);
+void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, ros::Publisher& pub);
+void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload, ros::Publisher& pub);
+void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload, ros::Publisher& pub);
+void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload, ros::Publisher& pub);
+void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload, ros::Publisher& pub);
+void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload, ros::Publisher& pub);
+void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload, ros::Publisher& pub);
+void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload, ros::Publisher& pub);
+
+void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, ros::Publisher& pub);
+
+void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub);
+void PublishOdometryData(const OdometryData& data, ros::Publisher& pub);
+void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub);
+
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-
-#endif
+#endif  // __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1_HPP__
diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp
index 58b5fad..ad99b4d 100644
--- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp
+++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp
@@ -1,141 +1,139 @@
 /**
- *  @file
- *  @brief Declaration of FixpositionDriver ROS1 Node
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Fixposition driver node for ROS1
  */
 
-#ifndef __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_
-#define __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_
-
-/* SYSTEM / STL */
-#include <termios.h>
+#ifndef __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_HPP__
+#define __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_HPP__
 
-/* ROS */
-#include <fixposition_driver_ros1/ros_msgs.hpp>
-#include <fixposition_driver_ros1/params.hpp>
+/* LIBC/STL */
+#include <memory>
+#include <mutex>
 
-/* FIXPOSITION */
+/* EXTERNAL */
 #include <fixposition_driver_lib/helper.hpp>
-#include <fixposition_driver_lib/gnss_tf.hpp>
+#include <fpsdk_ros1/ext/ros.hpp>
 
 /* PACKAGE */
-#include <fixposition_driver_ros1/data_to_ros1.hpp>
+#include "data_to_ros1.hpp"
+#include "params.hpp"
+#include "ros1_msgs.hpp"
 
 namespace fixposition {
-class FixpositionDriverNode : public FixpositionDriver {
+/* ****************************************************************************************************************** */
+
+class FixpositionDriverNode {
    public:
     /**
-     * @brief Construct a new Fixposition Driver Node object
+     * @brief Constructor
      *
-     * @param[in] params
+     * @param[in]  driver_params  Parameters
+     * @param[in]  node_params    Parameters
+     * @param[in]  nh             Node handle
      */
-    FixpositionDriverNode(const FixpositionDriverParams& params);
-
-    void Run();
+    FixpositionDriverNode(const DriverParams& driver_params, const NodeParams& node_params, ros::NodeHandle& nh);
 
-    void RegisterObservers();
-
-    void WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg);
-
-    void RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg);
+    /**
+     * @brief Destructor
+     */
+    ~FixpositionDriverNode();
 
-   private:
     /**
-     * @brief Observer Functions to publish NavSatFix from BestGnssPos
+     * @brief Start the node
      *
-     * @param[in] header
-     * @param[in] payload
+     * @returns true on success, false otherwise
      */
-    void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload);
+    bool StartNode();
 
     /**
-     * @brief Observer Function to publish NMEA message once the GNSS epoch transmission is complete
-     *
-     * @param[in] data
+     * @brief Stop the node
      */
-    void PublishNmea();
+    void StopNode();
 
-    // ROS node handler
-    ros::NodeHandle nh_;
-    
-    // ROS subscribers
-    ros::Subscriber ws_sub_;  //!< wheelspeed message subscriber
-    ros::Subscriber rtcm_sub_;  //!< RTCM3 message subscriber
+   private:
+    ros::NodeHandle nh_;          //!< ROS node handle
+    DriverParams driver_params_;  //!< Sensor/driver parameters
+    NodeParams node_params_;      //!< Node parameters
+    FixpositionDriver driver_;    //!< Sensor driver
 
     // ROS publishers
-    // FP_A messages
-    ros::Publisher fpa_odometry_pub_;    //!< FP_A-ODOMETRY message
-    ros::Publisher fpa_imubias_pub_;     //!< FP_A-IMUBIAS message
+    // - FP_A messages
     ros::Publisher fpa_eoe_pub_;         //!< FP_A-EOE message
+    ros::Publisher fpa_gnssant_pub_;     //!< FP_A-GNSSANT message
+    ros::Publisher fpa_gnsscorr_pub_;    //!< FP_A-GNSSCORR message
+    ros::Publisher fpa_imubias_pub_;     //!< FP_A-IMUBIAS message
     ros::Publisher fpa_llh_pub_;         //!< FP_A-LLH message
     ros::Publisher fpa_odomenu_pub_;     //!< FP_A-ODOMENU message
+    ros::Publisher fpa_odometry_pub_;    //!< FP_A-ODOMETRY message
     ros::Publisher fpa_odomsh_pub_;      //!< FP_A-ODOMSH message
     ros::Publisher fpa_odomstatus_pub_;  //!< FP_A-ODOMSTATUS message
-    ros::Publisher fpa_gnssant_pub_;     //!< FP_A-GNSSANT message
-    ros::Publisher fpa_gnsscorr_pub_;    //!< FP_A-GNSSCORR message
     ros::Publisher fpa_text_pub_;        //!< FP_A-TEXT message
     ros::Publisher fpa_tp_pub_;          //!< FP_A-TP message
+    // - NMEA messages
+    ros::Publisher nmea_gga_pub_;  //!< NMEA-GP-GGA message
+    ros::Publisher nmea_gll_pub_;  //!< NMEA-GP-GLL message
+    ros::Publisher nmea_gsa_pub_;  //!< NMEA-GN-GSA message
+    ros::Publisher nmea_gst_pub_;  //!< NMEA-GP-GST message
+    ros::Publisher nmea_gsv_pub_;  //!< NMEA-GX-GSV message
+    ros::Publisher nmea_hdt_pub_;  //!< NMEA-GP-HDT message
+    ros::Publisher nmea_rmc_pub_;  //!< NMEA-GP-RMC message
+    ros::Publisher nmea_vtg_pub_;  //!< NMEA-GP-VTG message
+    ros::Publisher nmea_zda_pub_;  //!< NMEA-GP-ZDA message
+    // - Odometry
+    ros::Publisher odometry_ecef_pub_;    //!< ECEF odometry
+    ros::Publisher odometry_enu_pub_;     //!< ENU odometry
+    ros::Publisher odometry_llh_pub_;     //!< LLH odometry
+    ros::Publisher odometry_smooth_pub_;  //!< Smooth Odometry (ECEF)
+    // - Orientation
+    ros::Publisher eul_pub_;      //!< Euler angles yaw-pitch-roll in local ENU
+    ros::Publisher eul_imu_pub_;  //!< Euler angles pitch-roll as estimated from the IMU in local horizontal
+    // - IMU
+    ros::Publisher corrimu_pub_;  //!< Bias corrected IMU data in IMU frame
+    ros::Publisher poiimu_pub_;   //!< Bias corrected IMU data in POI frame
+    ros::Publisher rawimu_pub_;   //!< Raw IMU data in IMU frame
+    // - GNSS
+    ros::Publisher navsatfix_gnss1_pub_;  //!< GNSS1 position and status
+    ros::Publisher navsatfix_gnss2_pub_;  //!< GNSS2 position and status
+    ros::Publisher nmea_epoch_pub_;       //!< NMEA epoch data
+    // - Other
+    ros::Publisher jump_pub_;  //!< Jump warning topic
+    ros::Publisher raw_pub_;   //!< Raw messages topic
 
-    // NMEA messages
-    ros::Publisher nmea_gpgga_pub_;      //!< NMEA-GP-GGA message
-    ros::Publisher nmea_gpgll_pub_;      //!< NMEA-GP-GLL message
-    ros::Publisher nmea_gngsa_pub_;      //!< NMEA-GP-GSA message
-    ros::Publisher nmea_gpgst_pub_;      //!< NMEA-GP-GST message
-    ros::Publisher nmea_gxgsv_pub_;      //!< NMEA-GP-GSV message
-    ros::Publisher nmea_gphdt_pub_;      //!< NMEA-GP-HDT message
-    ros::Publisher nmea_gprmc_pub_;      //!< NMEA-GP-RMC message
-    ros::Publisher nmea_gpvtg_pub_;      //!< NMEA-GP-VTG message
-    ros::Publisher nmea_gpzda_pub_;      //!< NMEA-GP-ZDA message
-
-    // ODOMETRY
-    ros::Publisher odometry_ecef_pub_;   //!< ECEF Odometry
-    ros::Publisher odometry_llh_pub_;    //!< LLH Odometry
-    ros::Publisher odometry_enu_pub_;    //!< ENU Odometry
-    ros::Publisher odometry_smooth_pub_; //!< Smooth Odometry (ECEF)
-    
-    // Orientation
-    ros::Publisher eul_pub_;             //!< Euler angles Yaw-Pitch-Roll in local ENU
-    ros::Publisher eul_imu_pub_;         //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal
-
-    // IMU
-    ros::Publisher rawimu_pub_;          //!< Raw IMU data in IMU frame
-    ros::Publisher corrimu_pub_;         //!< Bias corrected IMU data in IMU frame
-    ros::Publisher poiimu_pub_;          //!< Bias corrected IMU data in POI frame
-
-    // GNSS
-    ros::Publisher nmea_pub_;            //!< Pose estimation based only on GNSS
-    ros::Publisher navsatfix_gnss1_pub_; //!< GNSS1 position and status
-    ros::Publisher navsatfix_gnss2_pub_; //!< GNSS2 position and status
-    NmeaMessage nmea_message_;           //!< Collector class for NMEA messages
-    
-    // TF
-    tf2_ros::TransformBroadcaster br_;
-    tf2_ros::StaticTransformBroadcaster static_br_;
-
-    // Jump warning topic
-    ros::Publisher extras_jump_pub_;     //!< Jump warning topic
+    // ROS subscribers
+    ros::Subscriber ws_sub_;    //!< Wheelspeed input subscriber
+    ros::Subscriber corr_sub_;  //!< GNSS correction data input subscriber
 
-    // Previous state
-    Eigen::Vector3d prev_pos;
-    Eigen::MatrixXd prev_cov;
+    // TF2 broadcasters
+    tf2_ros::TransformBroadcaster tf_br_;
+    tf2_ros::StaticTransformBroadcaster static_br_;
 
-    // Nav2 TF map
-    std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>> tf_map = {
-        {"ECEFENU0", nullptr},
-        {"POIPOISH", nullptr},
-        {"ECEFPOISH", nullptr},
-        {"ENU0POI", nullptr}
+    // State
+    JumpDetector jump_detector_;
+    NmeaEpochData nmea_epoch_data_;  //!< NMEA collector
+
+    // TFs
+    struct Tfs {
+        std::mutex mutex_;
+        std::unique_ptr<geometry_msgs::TransformStamped> ecef_enu0_;
+        std::unique_ptr<geometry_msgs::TransformStamped> poi_poish_;
+        std::unique_ptr<geometry_msgs::TransformStamped> ecef_poish_;
+        std::unique_ptr<geometry_msgs::TransformStamped> enu0_poi_;
     };
+    Tfs tfs_;
+
+    void ProcessTfData(const TfData& tf_data);
+    void ProcessOdometryData(const OdometryData& odometry_data);
+    void PublishNav2Tf();
 };
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-
-#endif
+#endif  // __FIXPOSITION_DRIVER_ROS1_FIXPOSITION_DRIVER_NODE_HPP__
diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp
index ffeebbc..edf433c 100644
--- a/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp
+++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/params.hpp
@@ -1,58 +1,49 @@
 /**
- *  @file
- *  @brief Parameters
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Parameters
  */
 
 #ifndef __FIXPOSITION_DRIVER_ROS1_PARAMS_HPP__
 #define __FIXPOSITION_DRIVER_ROS1_PARAMS_HPP__
 
-/* ROS */
-#include <fixposition_driver_ros1/ros_msgs.hpp>
+/* LIBC/STL */
 
-/* FIXPOSITION */
+/* EXTERNAL */
 #include <fixposition_driver_lib/params.hpp>
 
+/* PACKAGE */
+
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 /**
- * @brief Load all parameters from ROS parameter server
+ * @brief Load sensor parameters from rosparam server
  *
- * @param[in] ns namespace to load the parameters from
- * @param[out] params
- * @return true
- * @return false
- */
-bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params);
-
-/**
- * @brief Load all parameters from ROS parameter server
+ * @param[in]  ns      Namespace to load the parameters from
+ * @param[out] params  The sensor parameters
  *
- * @param[in] ns namespace to load the parameters from
- * @param[out] params
- * @return true
- * @return false
+ * @returns true on success, false otherwise
  */
-bool LoadParamsFromRos1(const std::string& ns, CustomerInputParams& params);
+bool LoadParamsFromRos1(const std::string& ns, DriverParams& params);
 
 /**
- * @brief Load all parameters from ROS parameter server
+ * @brief Load node parameters from rosparam server
+ *
+ * @param[in]  ns      Namespace to load the parameters from
+ * @param[out] params  The sensor parameters
  *
- * @param[in] ns namespace to load the parameters from
- * @param[out] params
- * @return true
- * @return false
+ * @returns true on success, false otherwise
  */
-bool LoadParamsFromRos1(const std::string& ns, FixpositionDriverParams& params);
+bool LoadParamsFromRos1(const std::string& ns, NodeParams& params);
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-
-#endif
+#endif  // __FIXPOSITION_DRIVER_ROS1_PARAMS_HPP__
diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp
new file mode 100644
index 0000000..d73a740
--- /dev/null
+++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros1_msgs.hpp
@@ -0,0 +1,57 @@
+// Wrapper to suppress warnings from ROS headers
+#ifndef __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__
+#define __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wpedantic"
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#pragma GCC diagnostic ignored "-Wshadow"
+
+// Standard ROS messages
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <std_msgs/UInt8MultiArray.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+// RTCM
+#include <rtcm_msgs/Message.h>
+
+// Fixposition ROS messages
+// - Generic
+#include <fixposition_driver_msgs/NmeaEpoch.h>
+#include <fixposition_driver_msgs/ParserMsg.h>
+#include <fixposition_driver_msgs/Speed.h>
+// -Extras
+#include <fixposition_driver_msgs/CovWarn.h>
+// - FP-A
+#include <fixposition_driver_msgs/FpaConsts.h>
+#include <fixposition_driver_msgs/FpaEoe.h>
+#include <fixposition_driver_msgs/FpaGnssant.h>
+#include <fixposition_driver_msgs/FpaGnsscorr.h>
+#include <fixposition_driver_msgs/FpaImubias.h>
+#include <fixposition_driver_msgs/FpaLlh.h>
+#include <fixposition_driver_msgs/FpaOdomenu.h>
+#include <fixposition_driver_msgs/FpaOdometry.h>
+#include <fixposition_driver_msgs/FpaOdomsh.h>
+#include <fixposition_driver_msgs/FpaOdomstatus.h>
+#include <fixposition_driver_msgs/FpaText.h>
+#include <fixposition_driver_msgs/FpaTp.h>
+// - NMEA
+#include <fixposition_driver_msgs/NmeaGga.h>
+#include <fixposition_driver_msgs/NmeaGll.h>
+#include <fixposition_driver_msgs/NmeaGsa.h>
+#include <fixposition_driver_msgs/NmeaGst.h>
+#include <fixposition_driver_msgs/NmeaGsv.h>
+#include <fixposition_driver_msgs/NmeaHdt.h>
+#include <fixposition_driver_msgs/NmeaRmc.h>
+#include <fixposition_driver_msgs/NmeaVtg.h>
+#include <fixposition_driver_msgs/NmeaZda.h>
+
+#pragma GCC diagnostic pop
+#endif  // __FIXPOSITION_DRIVER_ROS1_ROS_MSGS_HPP__
diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp
deleted file mode 100644
index 03c5e3d..0000000
--- a/fixposition_driver_ros1/include/fixposition_driver_ros1/ros_msgs.hpp
+++ /dev/null
@@ -1,66 +0,0 @@
-// Wrapper to include ROS stuff without quoting the same warning suppressions all over
-#ifndef __ROS1_DRIVER_ROS_MSGS_HPP__
-#define __ROS1_DRIVER_ROS_MSGS_HPP__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wpedantic"
-#pragma GCC diagnostic ignored "-Wunused-parameter"
-#pragma GCC diagnostic ignored "-Wshadow"
-
-#include <ros/ros.h>
-#include <ros/console.h>
-
-#include <std_msgs/UInt8MultiArray.h>
-
-#include <nav_msgs/Odometry.h>
-
-#include <sensor_msgs/Imu.h>
-#include <sensor_msgs/NavSatFix.h>
-
-#include <geometry_msgs/Vector3Stamped.h>
-#include <geometry_msgs/TransformStamped.h>
-
-#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2/LinearMath/Transform.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-
-// Include generic
-#include <fixposition_driver_ros1/Speed.h>
-#include <fixposition_driver_ros1/GnssSats.h>
-#include <fixposition_driver_ros1/NMEA.h>
-
-// Include RTCM
-#include <rtcm_msgs/Message.h>
-
-// Include extras
-#include <fixposition_driver_ros1/CovWarn.h>
-
-// Include FP-A
-#include <fixposition_driver_ros1/gnssant.h>
-#include <fixposition_driver_ros1/gnsscorr.h>
-#include <fixposition_driver_ros1/imubias.h>
-#include <fixposition_driver_ros1/eoe.h>
-#include <fixposition_driver_ros1/llh.h>
-#include <fixposition_driver_ros1/odomenu.h>
-#include <fixposition_driver_ros1/odometry.h>
-#include <fixposition_driver_ros1/odomsh.h>
-#include <fixposition_driver_ros1/odomstatus.h>
-#include <fixposition_driver_ros1/text.h>
-#include <fixposition_driver_ros1/tp.h>
-
-// Include NMEA
-#include <fixposition_driver_ros1/gpgga.h>
-#include <fixposition_driver_ros1/gpgll.h>
-#include <fixposition_driver_ros1/gngsa.h>
-#include <fixposition_driver_ros1/gpgst.h>
-#include <fixposition_driver_ros1/gxgsv.h>
-#include <fixposition_driver_ros1/gphdt.h>
-#include <fixposition_driver_ros1/gprmc.h>
-#include <fixposition_driver_ros1/gpvtg.h>
-#include <fixposition_driver_ros1/gpzda.h>
-
-#include <eigen_conversions/eigen_msg.h>
-
-#pragma GCC diagnostic pop
-#endif  // __ROS1_DRIVER_ROS_MSGS_HPP__
diff --git a/fixposition_driver_ros1/launch/config.yaml b/fixposition_driver_ros1/launch/config.yaml
new file mode 100644
index 0000000..ba76671
--- /dev/null
+++ b/fixposition_driver_ros1/launch/config.yaml
@@ -0,0 +1,58 @@
+# Configuration for the Vision-RTK 2 sensor
+driver:
+    # Connection to the sensor one of:
+    # - Serial (<device>:<baudrate>)
+    # stream: serial:///dev/ttyUSB0:115200
+    # - TCP client (<ip_address>:<port>)
+    stream: tcpcli://172.22.1.44:21000
+    # Wait time in [s] to attempt reconnecting after connection lost
+    reconnect_delay: 5.0
+
+    # Messages that should be used by the driver. Note that the sensor must be configured accordingly for the correct
+    # port used in the "connection" above.
+    messages:
+        ##### Fusion output
+        # - Odometry and status
+        - "FP_A-ODOMETRY"          # configuration: FP_A-ODOMETRY
+        - "FP_A-ODOMENU"           # configuration: FP_A-ODOMENU
+        - "FP_A-ODOMSH"            # configuration: FP_A-ODOMSH
+        - "FP_A-ODOMSTATUS"        # configuration: FP_A-ODOMSTATUS
+        - "FP_A-IMUBIAS"           # configuration: FP_A-IMUBIAS
+        - "FP_A-EOE"               # configuration: FP_A-EOE_FUSION and FP_A-EOE_<epoch>
+        # - Transforms
+        - "FP_A-TF"                # configuration: FP_A-TF_{POIIMUH,VRTKCAM,POIVRTK,ECEFENU0,POIPOISH}
+        # - IMU data
+        - "FP_A-RAWIMU"            # configuration: FP_A-RAWIMU
+        - "FP_A-CORRIMU"           # configuration: FP_A-CORRIMU
+        # - LLH output
+        - "FP_A-LLH"               # configuration: FP_A-LLH
+        ##### Info messages
+        - "FP_A-TEXT"              # configuration: FP_A-TEXT_{ERROR,WARNING,INFO,DEBUG} (one or more)
+        ##### GNSS related data
+        - "FP_A-GNSSANT"           # configuration: FP_A-GNSSANT
+        - "FP_A-GNSSCORR"          # configuration: FP_A-GNSSCORR
+        - "FP_A-TP"                # configuration: FP_A-TP
+        ##### NMEA (see also nmea_epoch param below)
+        - "GGA"                    # configuration: NMEA-GN-GGA_<epoch> (or NMEA-GP-GGA_<epoch>)
+        - "GLL"                    # configuration: NMEA-GN-GLL_<epoch> (or NMEA-GP-GLL_<epoch>)
+        - "GSA"                    # configuration: NMEA-GN-GSA_<epoch>
+        - "GST"                    # configuration: NMEA-GN-GST_<epoch> (or NMEA-GP-GST_<epoch>)
+        - "GSV"                    # configuration: NMEA-GX-GSV_<epoch>
+        - "HDT"                    # configuration: NMEA-GN-HDT_<epoch> (or NMEA-GP-HDT_<epoch>)
+        - "RMC"                    # configuration: NMEA-GN-RMC_<epoch> (or NMEA-GP-RMC_<epoch>)
+        - "VTG"                    # configuration: NMEA-GN-VTG_<epoch> (or NMEA-GP-VTG_<epoch>)
+        - "ZDA"                    # configuration: NMEA-GN-ZDA_<epoch> (or NMEA-GP-ZDA_<epoch>)
+        ##### GNSS only position
+        - "NOV_B-BESTGNSSPOS"   # This can be one or both of the _GNSS1 and _GNSS2 variants
+
+    # Driver behaviour
+    nmea_epoch:  "GNSS"   # Choice for NMEA collection, must match NMEA message configuration type (<epoch> above)
+    raw_output:  false    # Enable raw messages output
+    cov_warning: false    # Enable covariance warnings
+    nav2_mode:   true     # Enable nav2 mode
+
+# Node parameters
+node:
+    output_ns:   "/fixposition"              # Namespace for output topics, leave empty to use node namespace
+    speed_topic: "/fixposition/speed"        # Wheelspeed input, empty to disable
+    corr_topic:  "/rtcm"                     # Correction data input, empty to disable
diff --git a/fixposition_driver_ros1/launch/dev.launch b/fixposition_driver_ros1/launch/dev.launch
new file mode 100644
index 0000000..e27f4cd
--- /dev/null
+++ b/fixposition_driver_ros1/launch/dev.launch
@@ -0,0 +1,9 @@
+<launch>
+    <arg name="node_name"   default="fixposition_driver_ros1" doc="Node name"/>
+    <arg name="config"      default="config.yaml"             doc="Configuration file to use"/>
+    <arg name="launcher"    default=""                        doc="Launch node via this (node launch-prefix)"/>
+    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find fixposition_driver_ros1)/launch/rosconsole_dev.conf"/>
+    <node name="$(arg node_name)" pkg="fixposition_driver_ros1" type="fixposition_driver_ros1" output="screen" respawn="false" launch-prefix="$(arg launcher)">
+        <rosparam command="load" file="$(find fixposition_driver_ros1)/launch/$(arg config)"/>
+    </node>
+</launch>
diff --git a/fixposition_driver_ros1/launch/driver_with_odom_converter.launch b/fixposition_driver_ros1/launch/driver_with_odom_converter.launch
index bf789cf..3db8310 100644
--- a/fixposition_driver_ros1/launch/driver_with_odom_converter.launch
+++ b/fixposition_driver_ros1/launch/driver_with_odom_converter.launch
@@ -1,6 +1,4 @@
 <launch>
-    <!-- Select TCP or Serial Accordingly  -->
-    <include file="$(find fixposition_driver_ros1)/launch/tcp.launch"/> 
-    <!-- <include file="$(find fixposition_driver_ros1)/launch/tcp.launch"/>  -->
+    <include file="$(find fixposition_driver_ros1)/launch/node.launch"/>
     <include file="$(find fixposition_odometry_converter_ros1)/launch/odom_converter.launch"/>
 </launch>
diff --git a/fixposition_driver_ros1/launch/node.launch b/fixposition_driver_ros1/launch/node.launch
new file mode 100644
index 0000000..9cf67aa
--- /dev/null
+++ b/fixposition_driver_ros1/launch/node.launch
@@ -0,0 +1,9 @@
+<launch>
+    <arg name="node_name"   default="fixposition_driver_ros1" doc="Node name"/>
+    <arg name="config"      default="config.yaml"             doc="Configuration file to use"/>
+    <arg name="launcher"    default=""                        doc="Launch node via this (node launch-prefix)"/>
+    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find fixposition_driver_ros1)/launch/rosconsole.conf"/>
+    <node name="$(arg node_name)" pkg="fixposition_driver_ros1" type="fixposition_driver_ros1" output="screen" respawn="true" respawn_delay="5" launch-prefix="$(arg launcher)">
+        <rosparam command="load" file="$(find fixposition_driver_ros1)/launch/$(arg config)"/>
+    </node>
+</launch>
diff --git a/fixposition_driver_ros1/launch/rosconsole.conf b/fixposition_driver_ros1/launch/rosconsole.conf
index 36d81cb..a7a64e4 100644
--- a/fixposition_driver_ros1/launch/rosconsole.conf
+++ b/fixposition_driver_ros1/launch/rosconsole.conf
@@ -1,2 +1,4 @@
-# Set the default ros output to info and higher
+#  General/main ROS logger
 log4j.logger.ros=INFO
+# - Fixposition ROS driver (fixposition_driver_ros1 and fpsdk::common::logging)
+log4j.logger.ros.fixposition_driver_ros1=INFO
diff --git a/fixposition_driver_ros1/launch/rosconsole_dev.conf b/fixposition_driver_ros1/launch/rosconsole_dev.conf
new file mode 100644
index 0000000..af801d5
--- /dev/null
+++ b/fixposition_driver_ros1/launch/rosconsole_dev.conf
@@ -0,0 +1,2 @@
+log4j.logger.ros=INFO
+log4j.logger.ros.fixposition_driver_ros1=DEBUG
diff --git a/fixposition_driver_ros1/launch/serial.launch b/fixposition_driver_ros1/launch/serial.launch
deleted file mode 100644
index d6fe44f..0000000
--- a/fixposition_driver_ros1/launch/serial.launch
+++ /dev/null
@@ -1,16 +0,0 @@
-<launch>
-    <!--Change input_type and input_port according to the type of input source:
-               tcp - data over TCP
-               serial - data over a serial port
-        And according to the port used for reading, i.e.
-               21000 - TCP port
-               /dev/ttyUSB0 - serial port -->
-    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find fixposition_driver_ros1)/launch/rosconsole.conf"/>
-
-
-    <node name="fixposition_driver_ros1" pkg="fixposition_driver_ros1" type="fixposition_driver_ros1" output="screen" respawn="true" respawn_delay="5">
-        <rosparam command="load" file="$(find fixposition_driver_ros1)/launch/serial.yaml" />
-
-
-    </node>
-</launch>
diff --git a/fixposition_driver_ros1/launch/serial.yaml b/fixposition_driver_ros1/launch/serial.yaml
deleted file mode 100644
index 1db13c7..0000000
--- a/fixposition_driver_ros1/launch/serial.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-fp_output:
-    formats: [
-              "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", 
-              "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A
-              "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA
-             ]
-    type: serial
-    port: "/dev/ttyUSB0"
-    baudrate: 115200
-    rate: 200
-    reconnect_delay: 5.0 # wait time in [s] until retry connection
-    cov_warning: false
-    nav2_mode: false
-
-customer_input:
-    speed_topic: "/fixposition/speed"
-    rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros1/launch/tcp.launch b/fixposition_driver_ros1/launch/tcp.launch
deleted file mode 100644
index fb1c88b..0000000
--- a/fixposition_driver_ros1/launch/tcp.launch
+++ /dev/null
@@ -1,16 +0,0 @@
-<launch>
-    <!--Change input_type and input_port according to the type of input source:
-               tcp - data over TCP
-               serial - data over a serial port
-        And according to the port used for reading, i.e.
-               21000 - TCP port
-               /dev/ttyUSB0 - serial port -->
-    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find fixposition_driver_ros1)/launch/rosconsole.conf"/>
-
-
-    <node name="fixposition_driver_ros1" pkg="fixposition_driver_ros1" type="fixposition_driver_ros1" output="screen" respawn="true" respawn_delay="5">
-        <rosparam command="load" file="$(find fixposition_driver_ros1)/launch/tcp.yaml" />
-
-
-    </node>
-</launch>
diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml
deleted file mode 100644
index 1f7745f..0000000
--- a/fixposition_driver_ros1/launch/tcp.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-fp_output:
-    formats: [
-              "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", 
-              "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A
-              "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA
-             ]
-    type: tcp
-    port: "21000"
-    ip: "10.0.2.1" # change to VRTK2's IP address in the network
-    rate: 200
-    reconnect_delay: 5.0 # wait time in [s] until retry connection
-    cov_warning: false
-    nav2_mode: false
-
-customer_input:
-    speed_topic: "/fixposition/speed"
-    rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros1/msg/GnssSats.msg b/fixposition_driver_ros1/msg/GnssSats.msg
deleted file mode 100644
index a965233..0000000
--- a/fixposition_driver_ros1/msg/GnssSats.msg
+++ /dev/null
@@ -1,23 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition GNSS satellite signal statistics message.
-#
-#
-####################################################################################################
-
-# Format | Field         | Unit    | Description
-# -------|---------------|---------|------------------------------------|
-string     constellation # [Hex]   | Signal ID (see below).
-int16[]    sat_id        # [-]     | Satellite ID number.
-int16[]    azim          # [deg]   | Satellite azimuth from true North.
-int16[]    elev          # [deg]   | Satellite elevation.
-int16[]    cno_l1        # [db-Hz] | Satellite SNR (C/No) for L1-band.
-int16[]    cno_l2        # [db-Hz] | Satellite SNR (C/No) for L2-band.
diff --git a/fixposition_driver_ros1/msg/NMEA.msg b/fixposition_driver_ros1/msg/NMEA.msg
deleted file mode 100644
index 31d883a..0000000
--- a/fixposition_driver_ros1/msg/NMEA.msg
+++ /dev/null
@@ -1,65 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA Message. Specified using the WGS 84 reference ellipsoid.
-#
-#
-####################################################################################################
-
-# Format  | Field       | Unit              | Description
-# --------|-------------|-------------------|----------------------------------------------------------------------|
-Header      header      # [ns]              | Specifies the ROS time and Euclidian reference frame.
-string      time        # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-string      date        # [ddmmyy]          | UTC date (day, month and year).
-float64     latitude    # [ddmm.mmmmm(mm)]  | Latitude. Positive is north of equator; negative is south.
-float64     longitude   # [dddmm.mmmmm(mm)] | Longitude. Positive is east of prime meridian; negative is west.
-float64     altitude    # [m]               | Altitude. Positive is above the WGS 84 ellipsoid.
-int8        quality     # [-]               | Quality indicator (see below).
-int8        num_sv      # [-]               | Number of satellites. Strict NMEA: 00-12. High-precision NMEA: 00-99.
-int8[]      ids         # [-]               | ID numbers of satellites used in solution. See the NMEA 0183 version 4.11 standard document.
-float64     hdop_rec    # [0.10-99.99]      | Horizontal dilution of precision.
-float64     pdop        # [-]               | Position dillution of precision.
-float64     hdop        # [-]               | Horizontal dillution of precision.
-float64     vdop        # [-]               | Vertical dillution of precision.
-float64     rms_range   # [-]               | RMS value of the standard deviation of the range inputs to the navigation process.
-float64     std_major   # [m]               | Standard deviation of semi-major axis of error ellipse.
-float64     std_minor   # [m]               | Standard deviation of semi-minor axis of error ellipse.
-float64     angle_major # [deg]             | Angle of semi-major axis of error ellipse from true North.
-float64     std_lat     # [m]               | Standard deviation of latitude error.
-float64     std_lon     # [m]               | Standard deviation of longitude error.
-float64     std_alt     # [m]               | Standard deviation of altitude error.
-float64[9]  covariance  # [m2]              | Position covariance defined relative to a tangential plane (ENU frame).
-int8        cov_type    # [-]               | Method employed to estimate covariance (see below).
-float64     heading     # [deg]             | True heading.
-float64     speed       # [m/s]             | Speed over ground.
-float64     course      # [deg]             | Course over ground (w.r.t. True North).
-float64     diff_age    # [-]               | Approximate age of differential data (last GPS MSM message received).
-string      diff_sta    # [-]               | DGPS station ID (0000-1023).
-fixposition_driver_ros1/GnssSats[] gnss_sats # [-]| GNSS satellite signal statistics.
-
-# Quality indicator table
-#
-# | ID | Signal         |
-# |----|----------------|
-# |  0 | Invalid        |
-# |  1 | Non-RTK fix    |
-# |  4 | RTK fixed      |
-# |  5 | RTK float      |
-# |  6 | Dead-reckoning |
-
-
-# Covariance type table
-#
-# | ID | Signal         |
-# |----|----------------|
-# |  0 | Unknown        |
-# |  1 | Approximated   |
-# |  2 | Diagonal known |
-# |  3 | Known          |
diff --git a/fixposition_driver_ros1/msg/Speed.msg b/fixposition_driver_ros1/msg/Speed.msg
deleted file mode 100644
index d8efeab..0000000
--- a/fixposition_driver_ros1/msg/Speed.msg
+++ /dev/null
@@ -1,17 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Wheel speed input to Fixposition ROS Driver
-#
-#
-####################################################################################################
-
-# Velocity values for one or several WheelSensors
-fixposition_driver_ros1/WheelSensor[] sensors
diff --git a/fixposition_driver_ros1/msg/WheelSensor.msg b/fixposition_driver_ros1/msg/WheelSensor.msg
deleted file mode 100644
index 33c7552..0000000
--- a/fixposition_driver_ros1/msg/WheelSensor.msg
+++ /dev/null
@@ -1,29 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Individual wheelspeed measurement
-#
-#
-####################################################################################################
-# Standard metadata
-std_msgs/Header header
-
-# Location of the wheelspeed measurement (one of: RC, FR, FL, RR, RL)
-string location
-
-# Velocity values in [mm/s] as integer 32bit
-int32 vx
-int32 vy
-int32 vz
-
-# Velocity validity
-bool vx_valid
-bool vy_valid
-bool vz_valid
diff --git a/fixposition_driver_ros1/msg/extras/CovWarn.msg b/fixposition_driver_ros1/msg/extras/CovWarn.msg
deleted file mode 100644
index 3517ad8..0000000
--- a/fixposition_driver_ros1/msg/extras/CovWarn.msg
+++ /dev/null
@@ -1,18 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition Covariance Warning Message
-#
-#
-####################################################################################################
-
-Header header
-geometry_msgs/Vector3 jump         # Position change
-geometry_msgs/Vector3 covariance   # Covariance
diff --git a/fixposition_driver_ros1/msg/fpa/eoe.msg b/fixposition_driver_ros1/msg/fpa/eoe.msg
deleted file mode 100644
index f2a1f78..0000000
--- a/fixposition_driver_ros1/msg/fpa/eoe.msg
+++ /dev/null
@@ -1,17 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-EOE Message
-#
-#
-####################################################################################################
-
-Header header
-string epoch   # Indicates which epoch ended (FUSION, GNSS, GNSS1 or GNSS2)
diff --git a/fixposition_driver_ros1/msg/fpa/gnssant.msg b/fixposition_driver_ros1/msg/fpa/gnssant.msg
deleted file mode 100644
index 37809f7..0000000
--- a/fixposition_driver_ros1/msg/fpa/gnssant.msg
+++ /dev/null
@@ -1,22 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-GNSSANT Message
-#
-#
-####################################################################################################
-
-Header header
-string gnss1_state   # GNSS1 antenna state
-string gnss1_power   # GNSS1 antenna power
-int32  gnss1_age     # Time since gnss1_state or gnss1_power information last changed
-string gnss2_state   # GNSS2 antenna state
-string gnss2_power   # GNSS2 antenna power
-int32  gnss2_age     # Time since gnss2_state or gnss2_power information last changed
diff --git a/fixposition_driver_ros1/msg/fpa/gnsscorr.msg b/fixposition_driver_ros1/msg/fpa/gnsscorr.msg
deleted file mode 100644
index d465731..0000000
--- a/fixposition_driver_ros1/msg/fpa/gnsscorr.msg
+++ /dev/null
@@ -1,31 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-GNSSCORR Message
-#
-#
-####################################################################################################
-
-Header header
-int16 gnss1_fix          # GNSS1 fix status
-int16 gnss1_nsig_l1      # Number of L1 signals with correction data tracked by GNSS1
-int16 gnss1_nsig_l2      # Number of L2 signals with correction data tracked by GNSS1
-int16 gnss2_fix          # GNSS2 fix status
-int16 gnss2_nsig_l1      # Number of L1 signals with correction data tracked by GNSS2
-int16 gnss2_nsig_l2      # Number of L2 signals with correction data tracked by GNSS2
-
-float64 corr_latency       # Average correction data latency (10s window)
-float64 corr_update_rate   # Average correction update rate (10s window)
-float64 corr_data_rate     # Average correction data rate (10s window)
-float64 corr_msg_rate      # Average correction message rate (10s window)
-
-int16 sta_id                    # Correction station ID, range 0–4095
-geometry_msgs/Vector3 sta_llh   # Correction station LLH position (latitude, longitude, height)
-int32 sta_dist                  # Correction station baseline length
diff --git a/fixposition_driver_ros1/msg/fpa/imubias.msg b/fixposition_driver_ros1/msg/fpa/imubias.msg
deleted file mode 100644
index 6eb123f..0000000
--- a/fixposition_driver_ros1/msg/fpa/imubias.msg
+++ /dev/null
@@ -1,71 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-IMUBIAS Message
-#
-#
-####################################################################################################
-
-Header header
-int16 fusion_imu                     # Fusion measurement status: IMU (see below)
-int16 imu_status                     # IMU bias status (see below)
-int16 imu_noise                      # IMU variance status (see below)
-int16 imu_conv                       # IMU convergence status (see below)
-geometry_msgs/Vector3 bias_acc       # Accelerometer bias
-geometry_msgs/Vector3 bias_gyr       # Gyroscope bias
-geometry_msgs/Vector3 bias_cov_acc   # Accelerometer bias covariance
-geometry_msgs/Vector3 bias_cov_gyr   # Gyroscope bias covariance
-
-
-# Fusion measurement status (fusion_imu)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not used           |
-# |   1   | Used               |
-# |   2   | Degraded           |
-
-
-# IMU bias status (imu_status)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not converged      |
-# |   1   | Warmstarted        |
-# |   2   | Rough convergence  |
-# |   3   | Fine convergence   |
-
-
-# IMU variance (imu_noise)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Reserved           |
-# |   1   | Low noise          |
-# |   2   | Medium noise       |
-# |   3   | High noise         |
-# | 4...7 | Reserved           |
-
-
-# IMU accelerometer and gyroscope convergence (imu_conv)
-#
-# | Value | Description                      |
-# |-------|----------------------------------|
-# |  null | Info not available               |
-# |   0   | Awaiting Fusion                  |
-# |   1   | Awaiting IMU measurements        |
-# |   2   | Insufficient global measurements |
-# |   3   | Insufficient motion              |
-# |   4   | Converging                       |
-# | 5...6 | Reserved                         |
-# |   7   | Idle                             |
\ No newline at end of file
diff --git a/fixposition_driver_ros1/msg/fpa/llh.msg b/fixposition_driver_ros1/msg/fpa/llh.msg
deleted file mode 100644
index ff9e44c..0000000
--- a/fixposition_driver_ros1/msg/fpa/llh.msg
+++ /dev/null
@@ -1,19 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-LLH Message
-#
-#
-####################################################################################################
-
-Header header
-string pose_frame                # frame of the pose values [pose, quaternion]
-geometry_msgs/Vector3 position   # LLH position (latitude, longitude, height)
-float64[9] covariance           # Position covariance in ENU
diff --git a/fixposition_driver_ros1/msg/fpa/odomenu.msg b/fixposition_driver_ros1/msg/fpa/odomenu.msg
deleted file mode 100644
index 6a4b2da..0000000
--- a/fixposition_driver_ros1/msg/fpa/odomenu.msg
+++ /dev/null
@@ -1,27 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMENU Message
-#
-#
-####################################################################################################
-
-Header header
-string pose_frame                            # frame of the pose values [pose, quaternion]
-string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
-geometry_msgs/PoseWithCovariance pose        # position, orientation
-geometry_msgs/TwistWithCovariance velocity   # linear, angular
-geometry_msgs/Vector3 acceleration           # linear acceleration
-
-int16 fusion_status                          # field for the fusion status
-int16 imu_bias_status                        # field for the IMU bias status
-int16 gnss1_status                           # field for the gnss1 status
-int16 gnss2_status                           # field for the gnss2 status
-int16 wheelspeed_status                      # field for the wheelspeed status
diff --git a/fixposition_driver_ros1/msg/fpa/odometry.msg b/fixposition_driver_ros1/msg/fpa/odometry.msg
deleted file mode 100644
index 6366cd0..0000000
--- a/fixposition_driver_ros1/msg/fpa/odometry.msg
+++ /dev/null
@@ -1,28 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMETRY Message
-#
-#
-####################################################################################################
-
-Header header
-string pose_frame                            # frame of the pose values [pose, quaternion]
-string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
-geometry_msgs/PoseWithCovariance pose        # position, orientation
-geometry_msgs/TwistWithCovariance velocity   # linear, angular
-geometry_msgs/Vector3 acceleration           # linear acceleration
-
-int16 fusion_status                          # field for the fusion status
-int16 imu_bias_status                        # field for the IMU bias status
-int16 gnss1_status                           # field for the gnss1 status
-int16 gnss2_status                           # field for the gnss2 status
-int16 wheelspeed_status                      # field for the wheelspeed status
-string version                               # Fixposition software version
diff --git a/fixposition_driver_ros1/msg/fpa/odomsh.msg b/fixposition_driver_ros1/msg/fpa/odomsh.msg
deleted file mode 100644
index 170183f..0000000
--- a/fixposition_driver_ros1/msg/fpa/odomsh.msg
+++ /dev/null
@@ -1,27 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMSH Message
-#
-#
-####################################################################################################
-
-Header header
-string pose_frame                            # frame of the pose values [pose, quaternion]
-string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
-geometry_msgs/PoseWithCovariance pose        # position, orientation
-geometry_msgs/TwistWithCovariance velocity   # linear, angular
-geometry_msgs/Vector3 acceleration           # linear acceleration
-
-int16 fusion_status                          # field for the fusion status
-int16 imu_bias_status                        # field for the IMU bias status
-int16 gnss1_status                           # field for the gnss1 status
-int16 gnss2_status                           # field for the gnss2 status
-int16 wheelspeed_status                      # field for the wheelspeed status
diff --git a/fixposition_driver_ros1/msg/fpa/odomstatus.msg b/fixposition_driver_ros1/msg/fpa/odomstatus.msg
deleted file mode 100644
index 3654ce6..0000000
--- a/fixposition_driver_ros1/msg/fpa/odomstatus.msg
+++ /dev/null
@@ -1,190 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMSTATUS Message
-#
-#
-####################################################################################################
-
-Header header
-int16 init_status                            # Fusion init status (see below)
-int16 fusion_imu                             # Fusion measurement status: IMU (see below)
-int16 fusion_gnss1                           # Fusion measurement status: GNSS 1 (see below)
-int16 fusion_gnss2                           # Fusion measurement status: GNSS 2 (see below)
-int16 fusion_corr                            # Fusion measurement status: GNSS corrections (see below)
-int16 fusion_cam1                            # Fusion measurement status: camera (see below)
-int16 fusion_ws                              # Fusion measurement status: wheelspeed (see below)
-int16 fusion_markers                         # Fusion measurement status: markers (see below)
-int16 imu_status                             # IMU bias status (see below)
-int16 imu_noise                              # IMU variance status (see below)
-int16 imu_conv                               # IMU convergence status (see below)
-int16 gnss1_status                           # GNSS 1 status (see below)
-int16 gnss2_status                           # GNSS 2 status (see below)
-int16 baseline_status                        # Baseline status (see below)
-int16 corr_status                            # GNSS correction status (see below)
-int16 cam1_status                            # Camera 1 status (see below)
-int16 ws_status                              # Wheelspeed status (see below)
-int16 ws_conv                                # Wheelspeed convergence status (see below)
-int16 markers_status                         # Markers status (see below)
-int16 markers_conv                           # Markers convergence status (see below)
-
-
-# Fusion initialisation status (init_status)
-#
-# | Value | Description          |
-# |-------|----------------------|
-# |  null | Unknown              |
-# |   0   | Not initialized      |
-# |   1   | Locally initialised  |
-# |   2   | Globally initialised |
-
-
-# Fusion measurement status (fusion_imu, fusion_cam1, fusion_cam2, fusion_gnss1, fusion_gnss2, fusion_corr, fusion_ws, fusion_markers)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not used           |
-# |   1   | Used               |
-# |   2   | Degraded           |
-
-
-# IMU bias status (imu_status)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not converged      |
-# |   1   | Warmstarted        |
-# |   2   | Rough convergence  |
-# |   3   | Fine convergence   |
-
-
-# IMU variance (imu_noise)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Reserved           |
-# |   1   | Low noise          |
-# |   2   | Medium noise       |
-# |   3   | High noise         |
-# | 4...7 | Reserved           |
-
-
-# IMU accelerometer and gyroscope convergence (imu_conv)
-#
-# | Value | Description                      |
-# |-------|----------------------------------|
-# |  null | Info not available               |
-# |   0   | Awaiting Fusion                  |
-# |   1   | Awaiting IMU measurements        |
-# |   2   | Insufficient global measurements |
-# |   3   | Insufficient motion              |
-# |   4   | Converging                       |
-# | 5...6 | Reserved                         |
-# |   7   | Idle                             |
-
-
-# GNSS fix status (gnss1_status, gnss2_status)
-#
-# | Value | Description                    |
-# |-------|--------------------------------|
-# |  null | Info not available             |
-# |   0   | No fix                         |
-# |   1   | Single-point positioning (SPP) |
-# |   2   | RTK moving baseline            |
-# | 3...4 | Reserved                       |
-# |   5   | RTK float                      |
-# | 6...7 | Reserved                       |
-# |   8   | RTK fixed                      |
-
-
-# GNSS correction status (corr_status)
-#
-# | Value | Description                                                                                                    |
-# |-------|----------------------------------------------------------------------------------------------------------------|
-# |  null | Info not available                                                                                             |
-# |   0   | Waiting fusion                                                                                                 |
-# |   1   | No GNSS available                                                                                              |
-# |   2   | No corrections used                                                                                            |
-# |   3   | Limited corrections used: station data & at least one of the constellations among the valid rover measurements |
-# |   4   | Corrections are too old                                                                                        |
-# |   5   | Sufficient corrections used: station data and corrections for ALL bands among the valid rover measurements     |
-
-
-# Baseline status (baseline_status)
-#
-# | Value | Description            |
-# |-------|------------------------|
-# |  null | Info not available     |
-# |   0   | Waiting fusion         |
-# |   1   | Not available / No fix |
-# |   2   | Failing                |
-# |   3   | Passing                |
-
-
-# Camera status (cam1_status)
-#
-# | Value | Description                                      |
-# |-------|--------------------------------------------------|
-# |  null | Info not available                               |
-# |   0   | Camera not available                             |
-# |   1   | Camera available, but not usable (e.g. too dark) |
-# | 2...4 | Reserved                                         |
-# |   5   | Camera working and available                     |
-
-
-# Wheelspeed status (ws_status)
-#
-# | Value | Description                                                |
-# |-------|------------------------------------------------------------|
-# |  null | Info not available                                         |
-# |   0   | No wheelspeed enabled                                      |
-# |   1   | Missing wheelspeed measurements                            |
-# |   2   | At least one wheelspeed enabled, no wheelspeed converged   |
-# |   3   | At least one wheelspeed enabled and at least one converged |
-# |   4   | At least one wheelspeed enabled and all converged          |
-
-
-# Wheelspeed convergence status (ws_conv)
-#
-# | Value | Description                       |
-# |-------|-----------------------------------|
-# |  null | Info not available                |
-# |   0   | Awaiting Fusion                   |
-# |   1   | Missing wheelspeed measurements   |
-# |   2   | Insufficient global measurements  |
-# |   3   | Insufficient motion               |
-# |   4   | Insufficient imu bias convergence |
-# |   5   | Converging                        |
-# |   6   | Idle                              |
-
-
-# Markers status (markers_status)
-#
-# | Value | Description                 |
-# |-------|-----------------------------|
-# |  null | Info not available          |
-# |   0   | No markers available        |
-# |   1   | Markers available           |
-# |   2   | Markers available, and used |
-
-
-# Markers convergence status (markers_conv)
-#
-# | Value | Description                      |
-# |-------|----------------------------------|
-# |  null | Info not available               |
-# |   0   | Awaiting Fusion                  |
-# |   1   | Waiting marker measurements      |
-# |   2   | Insufficient global measurements |
-# |   3   | Converging                       |
-# |   4   | Idle                             |
diff --git a/fixposition_driver_ros1/msg/fpa/text.msg b/fixposition_driver_ros1/msg/fpa/text.msg
deleted file mode 100644
index 50143fb..0000000
--- a/fixposition_driver_ros1/msg/fpa/text.msg
+++ /dev/null
@@ -1,18 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-TEXT Message
-#
-#
-####################################################################################################
-
-Header header
-string level   # One of: INFO, WARNING, ERROR, DEBUG
-string text    # Text
diff --git a/fixposition_driver_ros1/msg/fpa/tp.msg b/fixposition_driver_ros1/msg/fpa/tp.msg
deleted file mode 100644
index 6e12173..0000000
--- a/fixposition_driver_ros1/msg/fpa/tp.msg
+++ /dev/null
@@ -1,56 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-TP Message
-#
-#
-####################################################################################################
-
-string  tp_name                              # Timepulse name (source)
-string  timebase                             # Time base (see below), or null if not available
-string  timeref                              # Time reference (see below), or null if not available
-int64   tp_tow_sec                           # Timepulse time seconds of week, integer second part (0–604799), or null
-float64 tp_tow_psec                          # Timepulse time seconds of week, sub-second part (0.000000000000–0.999999999999), or null
-int64   gps_leaps                            # GPS leapseconds, or null if unknown
-
-
-# Values for timebase
-#
-# | Value | Description               |
-# |-------|---------------------------|
-# |  null | No timepulse alignment    |
-# |  GNSS | Timepulse aligned to GNSS |
-# |   UTC | Timepulse aligned to UTC  |
-
-
-# Values for timeref if timebase is GNSS
-#
-# | Value | Description                     |
-# |-------|---------------------------------|
-# |   GPS | Timepulse aligned to GPS        |
-# |   GAL | Timepulse aligned to Galileo    |
-# |   BDS | Timepulse aligned to BeiDou     |
-# |   GLO | Timepulse aligned to GLONASS    |
-# | OTHER | Timepulse aligned to other GNSS |
-
-
-# Values for timeref if timebase is UTC
-#
-# | Value | Description                                                                |
-# |-------|----------------------------------------------------------------------------|
-# |  NONE | Timepulse aligned to no UTC (no precise UTC parameters known yet)          |
-# |   CRL | Timepulse aligned to Communications Research Laboratory (CRL), Japan       |
-# |  NIST | Timepulse aligned to National Institute of Standards and Technology (NIST) |
-# |  USNO | Timepulse aligned to U.S. Naval Observatory (USNO)                         |
-# |  BIPM | Timepulse aligned to International Bureau of Weights and Measures (BIPM)   |
-# |    EU | Timepulse aligned to European laboratories                                 |
-# |    SU | Timepulse aligned to Former Soviet Union (SU)                              |
-# |  NTSC | Timepulse aligned to National Time Service Center (NTSC), China            |
-# | OTHER | Timepulse aligned to other/unknown UTC                                     |
diff --git a/fixposition_driver_ros1/msg/nmea/gngsa.msg b/fixposition_driver_ros1/msg/nmea/gngsa.msg
deleted file mode 100644
index f016494..0000000
--- a/fixposition_driver_ros1/msg/nmea/gngsa.msg
+++ /dev/null
@@ -1,24 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GN-GSA Message
-#
-#
-####################################################################################################
-
-# Format | Field    | Unit | Description
-# -------|----------|------|----------------------------------------------------------------------------------------------|
-char       mode_op  #  [-] | Operation mode: always A (automatic, allowed to automatically switch 2D/3D).
-int8       mode_nav #  [-] | Navigation mode: 1 (fix not available), 2 (2D) or 3 (3D).
-int8[]     ids      #  [-] | ID numbers of satellites used in solution. See the NMEA 0183 version 4.11 standard document.
-float64    pdop     #  [-] | Position dillution of precision.
-float64    hdop     #  [-] | Horizontal dillution of precision.
-float64    vdop     #  [-] | Vertical dillution of precision.
-int8       gnss_id  #  [-] | GNSS system ID: 1 (GPS, SBAS), 2 (GLONASS), 3 (Galileo), 4 (BeiDou), 5 (QZSS).
diff --git a/fixposition_driver_ros1/msg/nmea/gpgga.msg b/fixposition_driver_ros1/msg/nmea/gpgga.msg
deleted file mode 100644
index 817a0e4..0000000
--- a/fixposition_driver_ros1/msg/nmea/gpgga.msg
+++ /dev/null
@@ -1,40 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-GGA Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit              | Description
-# -------|-----------|-------------------|----------------------------------------------------------------------|
-string     time      # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-float64    latitude  # [ddmm.mmmmm(mm)]  | Latitude.
-char       lat_ns    # [-]               | Latitude north (N) or south (S) indicator.
-float64    longitude # [dddmm.mmmmm(mm)] | Longitude.
-char       lon_ew    # [-]               | Longitude east (E) or west (W) indicator.
-int8       quality   # [-]               | Quality indicator (see below).
-int8       num_sv    # [-]               | Number of satellites. Strict NMEA: 00-12. High-precision NMEA: 00-99.
-float64    hdop      # [0.10-99.99]      | Horizontal dilution of precision.
-float64    alt       # [m]               | Altitude (above ellipsoid).
-char       alt_unit  # [-]               | Altitude unit, always M (metres).
-float64    diff_age  # [-]               | Approximate age of differential data (last GPS MSM message received).
-string     diff_sta  # [-]               | DGPS station ID (0000-1023).
-string     sentence  # [-]               | ASCII string to be used by NTRIP clients.
-
-# Quality indicator table
-#
-# | ID | Signal         |
-# |----|----------------|
-# |  0 | Invalid        |
-# |  1 | Non-RTK fix    |
-# |  4 | RTK fixed      |
-# |  5 | RTK float      |
-# |  6 | Dead-reckoning |
diff --git a/fixposition_driver_ros1/msg/nmea/gpgll.msg b/fixposition_driver_ros1/msg/nmea/gpgll.msg
deleted file mode 100644
index efd712f..0000000
--- a/fixposition_driver_ros1/msg/nmea/gpgll.msg
+++ /dev/null
@@ -1,35 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-GLL Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit              | Description
-# -------|-----------|-------------------|--------------------------------------------------------|
-float64    latitude  # [ddmm.mmmmm(mm)]  | Latitude.
-char       lat_ns    # [-]               | Latitude north (N) or south (S) indicator.
-float64    longitude # [dddmm.mmmmm(mm)] | Longitude.
-char       lon_ew    # [-]               | Longitude east (E) or west (W) indicator.
-string     time      # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-char       status    # [-]               | Data validity status, A (data valid) or V (not valid).
-char       mode      # [-]               | Positioning system mode indicator (see below).
-
-# Mode table
-#
-# | ID | Signal                |
-# |----|-----------------------|
-# |  N | Data not valid        |
-# |  E | Dead-reckoning        |
-# |  D | Differential          |
-# |  A | Autonomous            |
-# |  M | Manual input not used |
-# |  S | Simulator not used    |
diff --git a/fixposition_driver_ros1/msg/nmea/gpgst.msg b/fixposition_driver_ros1/msg/nmea/gpgst.msg
deleted file mode 100644
index 4dd3193..0000000
--- a/fixposition_driver_ros1/msg/nmea/gpgst.msg
+++ /dev/null
@@ -1,25 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-GST Message
-#
-#
-####################################################################################################
-
-# Format | Field       | Unit            | Description
-# -------|-------------|-----------------|------------------------------------------------------------------------------------|
-string     time        # [hhmmss.ss(ss)] | UTC time (hours, minutes and seconds).
-float64    rms_range   # [-]             | RMS value of the standard deviation of the range inputs to the navigation process.
-float64    std_major   # [m]             | Standard deviation of semi-major axis of error ellipse.
-float64    std_minor   # [m]             | Standard deviation of semi-minor axis of error ellipse.
-float64    angle_major # [deg]           | Angle of semi-major axis of error ellipse from true North.
-float64    std_lat     # [m]             | Standard deviation of latitude error.
-float64    std_lon     # [m]             | Standard deviation of longitude error.
-float64    std_alt     # [m]             | Standard deviation of altitude error.
diff --git a/fixposition_driver_ros1/msg/nmea/gphdt.msg b/fixposition_driver_ros1/msg/nmea/gphdt.msg
deleted file mode 100644
index acad6da..0000000
--- a/fixposition_driver_ros1/msg/nmea/gphdt.msg
+++ /dev/null
@@ -1,19 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-HDT Message
-#
-#
-####################################################################################################
-
-# Format | Field    | Unit  | Description
-# -------|----------|-------|---------------|
-float64    heading  # [deg] | True heading.
-char       true_ind # [-]   | Always T.
\ No newline at end of file
diff --git a/fixposition_driver_ros1/msg/nmea/gprmc.msg b/fixposition_driver_ros1/msg/nmea/gprmc.msg
deleted file mode 100644
index 85a124d..0000000
--- a/fixposition_driver_ros1/msg/nmea/gprmc.msg
+++ /dev/null
@@ -1,38 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-RMC Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit              | Description
-# -------|-----------|-------------------|--------------------------------------------------------|
-string     time      # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-char       status    # [-]               | Data validity status, A (data valid) or V (not valid).
-float64    latitude  # [ddmm.mmmmm(mm)]  | Latitude.
-char       lat_ns    # [-]               | Latitude north (N) or south (S) indicator.
-float64    longitude # [dddmm.mmmmm(mm)] | Longitude.
-char       lon_ew    # [-]               | Longitude east (E) or west (W) indicator.
-float64    speed     # [knots]           | Speed over ground.
-float64    course    # [deg]             | Course over ground (w.r.t. True North).
-string     date      # [ddmmyy]          | UTC date (day, month and year).
-char       mode      # [-]               | Positioning system mode indicator (see below).
-
-# Mode table
-#
-# | ID | Signal                |
-# |----|-----------------------|
-# |  N | Data not valid        |
-# |  E | Dead-reckoning        |
-# |  D | Differential          |
-# |  A | Autonomous            |
-# |  M | Manual input not used |
-# |  S | Simulator not used    |
diff --git a/fixposition_driver_ros1/msg/nmea/gpvtg.msg b/fixposition_driver_ros1/msg/nmea/gpvtg.msg
deleted file mode 100644
index e3114f3..0000000
--- a/fixposition_driver_ros1/msg/nmea/gpvtg.msg
+++ /dev/null
@@ -1,37 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-VTG Message
-#
-#
-####################################################################################################
-
-# Format | Field      | Unit    | Description
-# -------|------------|---------|--------------------------------------------------------|
-float64    cog_true   # [deg]   | Course over ground wrt. True North.
-char       cog_ref_t  # [-]     | COG reference, always T (true).
-float64    cog_mag    # [-]     | Course over ground w.r.t. Magnetic North, always null.
-char       cog_ref_m  # [-]     | COG reference, always M (magnetic).
-float64    sog_knot   # [knots] | Speed over ground in knots.
-char       sog_unit_n # [-]     | SOG reference, always N (knots).
-float64    sog_kph    # [km/h]  | Speed over ground in km/h.
-char       sog_unit_k # [-]     | SOG reference, always K (km/h).
-char       mode       # [-]     | Positioning system mode indicator (see below).
-
-# Mode table
-#
-# | ID | Signal                |
-# |----|-----------------------|
-# |  N | Data not valid        |
-# |  E | Dead-reckoning        |
-# |  D | Differential          |
-# |  A | Autonomous            |
-# |  M | Manual input not used |
-# |  S | Simulator not used    |
diff --git a/fixposition_driver_ros1/msg/nmea/gpzda.msg b/fixposition_driver_ros1/msg/nmea/gpzda.msg
deleted file mode 100644
index 714958e..0000000
--- a/fixposition_driver_ros1/msg/nmea/gpzda.msg
+++ /dev/null
@@ -1,22 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-ZDA Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit            | Description
-# -------|-----------|-----------------|-------------------------------------------------------|
-Header     header    # [ns]            | Specifies the ROS time and Euclidian reference frame.
-string     time      # [hhmmss.ss(ss)] | UTC0 time (hours, minutes and seconds).
-string     date      # [dd/mm/yyyy]    | Date (UTC0).
-int16      local_hr  # [00+-13]        | Local zone hours, always 00 (= UTC).
-int16      local_min # [00-59]         | Local zone minutes, always 00 (= UTC).
diff --git a/fixposition_driver_ros1/msg/nmea/gxgsv.msg b/fixposition_driver_ros1/msg/nmea/gxgsv.msg
deleted file mode 100644
index a7ba73a..0000000
--- a/fixposition_driver_ros1/msg/nmea/gxgsv.msg
+++ /dev/null
@@ -1,39 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023  ___     ___
-#                       \\  \\  /  /
-#                        \\  \\/  /
-#                         /  /\\  \\
-#                        /__/  \\__\\  Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GX-GSV Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit    | Description
-# -------|-----------|---------|-------------------------------------|
-int16      sentences # [-]     | Total number of sentences (1 to 9).
-int16      sent_num  # [-]     | Sentence number (1 to 9).
-int32      num_sats  # [-]     | Total number of satellites in view.
-int16[]    sat_id    # [-]     | Satellite ID number.               
-int16[]    elev      # [deg]   | Satellite elevation. 
-int16[]    azim      # [deg]   | Satellite azimuth from true North.
-int16[]    cno       # [db-Hz] | Satellite SNR (C/No).
-string     signal_id # [Hex]   | Signal ID (see below).
-
-# Signal ID table
-#
-# | ID | Signal                        |
-# |----|-------------------------------|
-# |  0 | No signal (any talker ID)     |
-# |  1 | GPS/SBAS L1C/A (talker ID GP) |
-# |  6 | GPS L2C-L (talker ID GP)      |
-# |  7 | Galileo L1-BC (talker ID GA)  |
-# |  2 | Galileo E5b (talker ID GA)    |
-# |  1 | BeiDou B1I (talker ID GB)     |
-# |  B | BeiDou B2I (talker ID GB)     |
-# |  1 | GLONASS G1 C/A (talker ID GL) |
-# |  3 | GLONASS G2 C/A (talker ID GL) |
\ No newline at end of file
diff --git a/fixposition_driver_ros1/package.xml b/fixposition_driver_ros1/package.xml
index 7956e19..77b2d03 100644
--- a/fixposition_driver_ros1/package.xml
+++ b/fixposition_driver_ros1/package.xml
@@ -1,13 +1,10 @@
 <?xml version="1.0"?>
 <package format="2">
     <name>fixposition_driver_ros1</name>
-    <version>5.0.0</version>
-    <description>Fixposition ROS Driver</description>
-
-    <maintainer email="info@fixposition.com">Fixposition AG</maintainer>
-
+    <version>8.0.0</version>
+    <description>Fixposition ROS1 driver</description>
+    <maintainer email="support@fixposition.com">Fixposition Support</maintainer>
     <license>MIT</license>
-
     <buildtool_depend>catkin</buildtool_depend>
     <depend>roscpp</depend>
     <depend>tf</depend>
@@ -18,7 +15,10 @@
     <depend>message_generation</depend>
     <depend>message_runtime</depend>
     <depend>fixposition_driver_lib</depend>
+    <depend>fixposition_driver_msgs</depend>
     <depend>rtcm_msgs</depend>
     <depend>tf2_ros</depend>
     <depend>tf2_geometry_msgs</depend>
+    <depend>fpsdk_common</depend>
+    <depend>fpsdk_ros1</depend>
 </package>
diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp
index 7910e8f..bad6c35 100644
--- a/fixposition_driver_ros1/src/data_to_ros1.cpp
+++ b/fixposition_driver_ros1/src/data_to_ros1.cpp
@@ -1,811 +1,738 @@
 /**
- *  @file
- *  @brief Convert Data classes to ROS1 msgs
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Convert data to ROS1 msgs
  */
 
+/* LIBC/STL */
+#include <cmath>
+
+/* EXTERNAL */
+#include <fixposition_driver_msgs/data_to_ros.hpp>
+#include <fpsdk_common/math.hpp>
+#include <fpsdk_common/time.hpp>
+#include <fpsdk_common/trafo.hpp>
+#include <fpsdk_ros1/ext/eigen_conversions.hpp>
+#include <fpsdk_ros1/utils.hpp>
+
 /* PACKAGE */
-#include <fixposition_driver_ros1/data_to_ros1.hpp>
+#include "fixposition_driver_ros1/data_to_ros1.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-void FpToRosMsg(const OdometryData& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        nav_msgs::Odometry msg;
-        
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-        }
+using namespace fpsdk;
+using namespace fpsdk::common;
+using namespace fpsdk::common::parser;
 
-        msg.header.frame_id = data.frame_id;
-        msg.child_frame_id = data.child_frame_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        PoseWithCovDataToMsg(data.pose, msg.pose);
-        TwistWithCovDataToMsg(data.twist, msg.twist);
-        
-        // Publish message
-        pub.publish(msg);
-    }
+static void PoseWithCovDataToMsg(const PoseWithCovData& data, geometry_msgs::PoseWithCovariance& msg) {
+    tf::pointEigenToMsg(data.position, msg.pose.position);
+    tf::quaternionEigenToMsg(data.orientation, msg.pose.orientation);
+    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
+    cov_map = data.cov;
 }
 
-void FpToRosMsg(const ImuData& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        sensor_msgs::Imu msg;
-
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-        }
-        
-        msg.header.frame_id = data.frame_id;
-        tf::vectorEigenToMsg(data.linear_acceleration, msg.linear_acceleration);
-        tf::vectorEigenToMsg(data.angular_velocity, msg.angular_velocity);
-
-        // Publish message
-        pub.publish(msg);
-    }
+static void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWithCovariance& msg) {
+    tf::vectorEigenToMsg(data.linear, msg.twist.linear);
+    tf::vectorEigenToMsg(data.angular, msg.twist.angular);
+    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
+    cov_map = data.cov;
 }
 
-void FpToRosMsg(const FP_IMUBIAS& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::imubias msg;
-
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-        }
-        
-        msg.header.frame_id = data.frame_id;
-        msg.fusion_imu = data.fusion_imu;
-        msg.imu_status = data.imu_status;
-        msg.imu_noise = data.imu_noise;
-        msg.imu_conv = data.imu_conv;
-        tf::vectorEigenToMsg(data.bias_acc, msg.bias_acc);
-        tf::vectorEigenToMsg(data.bias_gyr, msg.bias_gyr);
-        tf::vectorEigenToMsg(data.bias_cov_acc, msg.bias_cov_acc);
-        tf::vectorEigenToMsg(data.bias_cov_gyr, msg.bias_cov_gyr);
-
-        // Publish message
-        pub.publish(msg);
-    }
+void TfDataToTransformStamped(const TfData& data, geometry_msgs::TransformStamped& msg) {
+    msg.header.stamp = ros1::utils::ConvTime(data.stamp);
+    msg.header.frame_id = data.frame_id;
+    msg.child_frame_id = data.child_frame_id;
+    tf::quaternionEigenToMsg(data.rotation, msg.transform.rotation);
+    tf::vectorEigenToMsg(data.translation, msg.transform.translation);
 }
 
-void FpToRosMsg(const FP_GNSSANT& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gnssant msg;
-        
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-        }
-
-        msg.gnss1_state = data.gnss1_state;
-        msg.gnss1_power = data.gnss1_power;
-        msg.gnss1_age = data.gnss1_age;
-        msg.gnss2_state = data.gnss2_state;
-        msg.gnss2_power = data.gnss2_power;
-        msg.gnss2_age = data.gnss2_age;
-        
-        // Publish message
-        pub.publish(msg);
-    }
+void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::TransformStamped& msg) {
+    msg.header.stamp = ros1::utils::ConvTime(data.stamp);
+    msg.header.frame_id = data.frame_id;
+    msg.child_frame_id = data.child_frame_id;
+    tf::quaternionEigenToMsg(data.pose.orientation, msg.transform.rotation);
+    tf::vectorEigenToMsg(data.pose.position, msg.transform.translation);
 }
 
-void FpToRosMsg(const FP_GNSSCORR& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gnsscorr msg;
-        
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-        }
+// ---------------------------------------------------------------------------------------------------------------------
 
-        msg.gnss1_fix = data.gnss1_fix;
-        msg.gnss1_nsig_l1 = data.gnss1_nsig_l1;
-        msg.gnss1_nsig_l2 = data.gnss1_nsig_l2;
-        msg.gnss2_fix = data.gnss2_fix;
-        msg.gnss2_nsig_l1 = data.gnss2_nsig_l1;
-        msg.gnss2_nsig_l2 = data.gnss2_nsig_l2;
+template <typename SomeFpaOdoPayload, typename SomeOdoMsg>
+static void FpaOdomToRos(const SomeFpaOdoPayload& payload, SomeOdoMsg& msg) {
+    msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
 
-        msg.corr_latency = data.corr_latency;
-        msg.corr_update_rate = data.corr_update_rate;
-        msg.corr_data_rate = data.corr_data_rate;
-        msg.corr_msg_rate = data.corr_msg_rate;
+    msg.fusion_status = FpaFusionStatusLegacyToMsg(msg, payload.fusion_status);
+    msg.imu_bias_status = FpaImuStatusLegacyToMsg(msg, payload.imu_bias_status);
+    msg.gnss1_status = FpaGnssFixToMsg(msg, payload.gnss1_fix);
+    msg.gnss2_status = FpaGnssFixToMsg(msg, payload.gnss2_fix);
+    msg.wheelspeed_status = FpaWsStatusLegacyToMsg(msg, payload.wheelspeed_status);
 
-        msg.sta_id = data.sta_id;
-        tf::vectorEigenToMsg(data.sta_llh, msg.sta_llh);
-        msg.sta_dist = data.sta_dist;
+    FpaFloat3ToVector3(payload.acc, msg.acceleration);
 
-        // Publish message
-        pub.publish(msg);
-    }
+    PoseWithCovData pose;
+    pose.SetFromFpaOdomPayload(payload);
+    PoseWithCovDataToMsg(pose, msg.pose);
+
+    TwistWithCovData velocity;
+    velocity.SetFromFpaOdomPayload(payload);
+    TwistWithCovDataToMsg(velocity, msg.velocity);
 }
 
-void FpToRosMsg(const FP_LLH& data, ros::Publisher& pub) {
+void PublishFpaOdometry(const fpa::FpaOdometryPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::llh msg;
-
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-        }
-
-        tf::vectorEigenToMsg(data.llh, msg.position);
-        Eigen::Map<Eigen::Matrix3d> cov_map = Eigen::Map<Eigen::Matrix3d>(msg.covariance.data());
-        cov_map = data.cov;
-
-        // Publish message
+        fixposition_driver_msgs::FpaOdometry msg;
+        msg.header.frame_id = ODOMETRY_FRAME_ID;
+        msg.pose_frame = ODOMETRY_CHILD_FRAME_ID;
+        msg.kin_frame = ODOMETRY_CHILD_FRAME_ID;
+        FpaOdomToRos(payload, msg);
+        msg.version = payload.version;
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMENU& data, ros::Publisher& pub) {
+void PublishFpaOdomenu(const fpa::FpaOdomenuPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::odomenu msg;
-
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp));
-        }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        msg.pose_frame = data.odom.child_frame_id;
-        msg.kin_frame = data.odom.child_frame_id;
-
-        PoseWithCovDataToMsg(data.odom.pose, msg.pose);
-        TwistWithCovDataToMsg(data.odom.twist, msg.velocity);
-        tf::vectorEigenToMsg(data.acceleration, msg.acceleration);
-        msg.fusion_status = data.fusion_status;
-        msg.imu_bias_status = data.imu_bias_status;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.wheelspeed_status = data.wheelspeed_status;
-
-        // Publish message
+        fixposition_driver_msgs::FpaOdomenu msg;
+        msg.header.frame_id = ODOMENU_FRAME_ID;
+        msg.pose_frame = ODOMENU_CHILD_FRAME_ID;
+        msg.kin_frame = ODOMENU_CHILD_FRAME_ID;
+        FpaOdomToRos(payload, msg);
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMETRY& data, ros::Publisher& pub) {
+void PublishFpaOdomsh(const fpa::FpaOdomshPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::odometry msg;
-
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp));
-        }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        msg.pose_frame = data.odom.child_frame_id;
-        msg.kin_frame = data.odom.child_frame_id;
-
-        PoseWithCovDataToMsg(data.odom.pose, msg.pose);
-        TwistWithCovDataToMsg(data.odom.twist, msg.velocity);
-        tf::vectorEigenToMsg(data.acceleration, msg.acceleration);
-        msg.fusion_status = data.fusion_status;
-        msg.imu_bias_status = data.imu_bias_status;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.wheelspeed_status = data.wheelspeed_status;
-        msg.version = data.version;
-        
-        // Publish message
+        fixposition_driver_msgs::FpaOdomsh msg;
+        msg.header.frame_id = ODOMSH_FRAME_ID;
+        msg.pose_frame = ODOMSH_CHILD_FRAME_ID;
+        msg.kin_frame = ODOMSH_CHILD_FRAME_ID;
+        FpaOdomToRos(payload, msg);
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMSH& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::odomsh msg;
-
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp));
-        }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        msg.pose_frame = data.odom.child_frame_id;
-        msg.kin_frame = data.odom.child_frame_id;
-
-        PoseWithCovDataToMsg(data.odom.pose, msg.pose);
-        TwistWithCovDataToMsg(data.odom.twist, msg.velocity);
-        tf::vectorEigenToMsg(data.acceleration, msg.acceleration);
-        msg.fusion_status = data.fusion_status;
-        msg.imu_bias_status = data.imu_bias_status;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.wheelspeed_status = data.wheelspeed_status;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        sensor_msgs::Imu msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = ODOMETRY_FRAME_ID;
+        FpaFloat3ToVector3(payload.acc, msg.linear_acceleration);
+        FpaFloat3ToVector3(payload.rot, msg.angular_velocity);
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMSTATUS& data, ros::Publisher& pub) {
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::odomstatus msg;
+        sensor_msgs::NavSatFix msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = ODOMSH_CHILD_FRAME_ID;
+
+        // Populate LLH position
+        PoseWithCovData pose;
+        pose.SetFromFpaOdomPayload(payload);
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
+            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
 
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
+        if (pose.position.isZero()) {
+            msg.position_covariance_type = msg.COVARIANCE_TYPE_UNKNOWN;
+            cov_map = Eigen::Matrix3d::Zero();  // FIXME: necessary?
         } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
+            const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(pose.position);
+            msg.latitude = math::RadToDeg(llh_pos(0));
+            msg.longitude = math::RadToDeg(llh_pos(1));
+            msg.altitude = llh_pos(2);
+
+            // Populate LLH covariance
+            const Eigen::Matrix3d p_cov_e = pose.cov.topLeftCorner(3, 3);
+            const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(pose.position);
+            const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose();
+            cov_map = p_cov_l;
+            msg.position_covariance_type = msg.COVARIANCE_TYPE_KNOWN;
         }
 
-        msg.init_status = data.init_status;
-        msg.fusion_imu = data.fusion_imu;
-        msg.fusion_gnss1 = data.fusion_gnss1;
-        msg.fusion_gnss2 = data.fusion_gnss2;
-        msg.fusion_corr = data.fusion_corr;
-        msg.fusion_cam1 = data.fusion_cam1;
-        msg.fusion_ws = data.fusion_ws;
-        msg.fusion_markers = data.fusion_markers;
-        msg.imu_status = data.imu_status;
-        msg.imu_noise = data.imu_noise;
-        msg.imu_conv = data.imu_conv;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.baseline_status = data.baseline_status;
-        msg.corr_status = data.corr_status;
-        msg.cam1_status = data.cam1_status;
-        msg.ws_status = data.ws_status;
-        msg.ws_conv = data.ws_conv;
-        msg.markers_status = data.markers_status;
-        msg.markers_conv = data.markers_conv;
+        // Populate LLH status
+        const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix);
+        msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix);
+        if (msg.status.status != msg.status.STATUS_NO_FIX) {
+            msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS |
+                                  msg.status.SERVICE_GALILEO);
+        }
 
         // Publish message
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_TEXT& data, ros::Publisher& pub) {
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishFpaOdomenuVector3Stamped(const fpa::FpaOdomenuPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::text msg;
+        geometry_msgs::Vector3Stamped msg;
+
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = ENU_FRAME_ID;
 
-        // Populate message
-        msg.level = data.level;
-        msg.text = data.text;
+        const Eigen::Quaterniond quat = {payload.orientation.values[0], payload.orientation.values[1],
+                                         payload.orientation.values[2], payload.orientation.values[3]};
+
+        // Euler angle wrt. ENU frame in the order of yaw pitch roll
+        const Eigen::Vector3d enu_euler = trafo::RotToEul(quat.toRotationMatrix());
+        // const Eigen::Vector3d enu_euler = trafo::QuatToEul(quat); // FIXME couldn't we just use this?
+        tf::vectorEigenToMsg(enu_euler, msg.vector);
 
         // Publish message
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_TP& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::tp msg;
-
-        // Populate message
-        msg.tp_name = data.tp_name;
-        msg.timebase = data.timebase;
-        msg.timeref = data.timeref;
-        msg.tp_tow_sec = data.tp_tow_sec;
-        msg.tp_tow_psec = data.tp_tow_psec;
-        msg.gps_leaps = data.gps_leaps;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaOdomstatus(const fpa::FpaOdomstatusPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::FpaOdomstatus msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+
+        // clang-format off
+        msg.init_status     = FpaInitStatusToMsg(msg, payload.init_status);
+        msg.fusion_imu      = FpaMeasStatusToMsg(msg, payload.fusion_imu);
+        msg.fusion_gnss1    = FpaMeasStatusToMsg(msg, payload.fusion_gnss1);
+        msg.fusion_gnss2    = FpaMeasStatusToMsg(msg, payload.fusion_gnss2);
+        msg.fusion_corr     = FpaMeasStatusToMsg(msg, payload.fusion_corr);
+        msg.fusion_cam1     = FpaMeasStatusToMsg(msg, payload.fusion_cam1);
+        msg.fusion_ws       = FpaMeasStatusToMsg(msg, payload.fusion_ws);
+        msg.fusion_markers  = FpaMeasStatusToMsg(msg, payload.fusion_markers);
+        msg.imu_status      = FpaImuStatusToMsg(msg, payload.imu_status);
+        msg.imu_noise       = FpaImuNoiseToMsg(msg, payload.imu_noise);
+        msg.imu_conv        = FpaImuConvToMsg(msg, payload.imu_conv);
+        msg.gnss1_status    = FpaGnssStatusToMsg(msg, payload.gnss1_status);
+        msg.gnss2_status    = FpaGnssStatusToMsg(msg, payload.gnss2_status);
+        msg.baseline_status = FpaBaselineStatusToMsg(msg, payload.baseline_status);
+        msg.corr_status     = FpaCorrStatusToMsg(msg, payload.corr_status);
+        msg.cam1_status     = FpaCamStatusToMsg(msg, payload.cam1_status);
+        msg.ws_status       = FpaWsStatusToMsg(msg, payload.ws_status);
+        msg.ws_conv         = FpaWsConvToMsg(msg, payload.ws_conv);
+        msg.markers_status  = FpaMarkersStatusToMsg(msg, payload.markers_status);
+        msg.markers_conv    = FpaMarkersConvToMsg(msg, payload.markers_conv);
+        // clang-format on
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_EOE& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::eoe msg;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
+void PublishFpaLlh(const fpa::FpaLlhPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::FpaLlh msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        FpaFloat3ToVector3(payload.llh, msg.position);
+        if (payload.cov_enu.valid) {
+            Eigen::Map<Eigen::Matrix3d> cov_map = Eigen::Map<Eigen::Matrix3d>(msg.covariance.data());
+            cov_map = BuildCovMat3D(payload.cov_enu.values[0], payload.cov_enu.values[1], payload.cov_enu.values[2],
+                                    payload.cov_enu.values[3], payload.cov_enu.values[4], payload.cov_enu.values[5]);
         }
-        msg.epoch = data.epoch;
-
-        // Publish message
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_GGA& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gpgga msg;
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.latitude = data.llh(0);
-        msg.lat_ns = data.lat_ns;
-        msg.longitude = data.llh(1);
-        msg.lon_ew = data.lon_ew;
-        msg.quality = data.quality;
-        msg.num_sv = data.num_sv;
-        msg.hdop = data.hdop;
-        msg.alt = data.llh(2);
-        msg.alt_unit = data.alt_unit;
-        msg.diff_age = data.diff_age;
-        msg.diff_sta = data.diff_sta;
-        msg.sentence = data.sentence;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaEoe(const fpa::FpaEoePayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::FpaEoe msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.epoch = FpaEpochToMsg(msg, payload.epoch);
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_GLL& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gpgll msg;
-
-        // Populate message
-        msg.latitude = data.latlon(0);
-        msg.lat_ns = data.lat_ns;
-        msg.longitude = data.latlon(1);
-        msg.lon_ew = data.lon_ew;
-        msg.time = data.time_str;
-        msg.status = data.status;
-        msg.mode = data.mode;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaImubias(const fpa::FpaImubiasPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::FpaImubias msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = IMU_FRAME_ID;
+        msg.fusion_imu = FpaMeasStatusToMsg(msg, payload.fusion_imu);
+        msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status);
+        msg.imu_noise = FpaImuNoiseToMsg(msg, payload.imu_noise);
+        msg.imu_conv = FpaImuConvToMsg(msg, payload.imu_conv);
+        FpaFloat3ToVector3(payload.bias_acc, msg.bias_acc);
+        FpaFloat3ToVector3(payload.bias_gyr, msg.bias_gyr);
+        FpaFloat3ToVector3(payload.bias_cov_acc, msg.bias_cov_acc);
+        FpaFloat3ToVector3(payload.bias_cov_gyr, msg.bias_cov_gyr);
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const GN_GSA& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gngsa msg;
-
-        // Populate message
-        msg.mode_op = data.mode_op;
-        msg.mode_nav = data.mode_nav;
-        
-        for (unsigned int i = 0; i < data.ids.size(); i++) {
-           msg.ids.push_back(data.ids.at(i));
-        }
-        
-        msg.pdop = data.pdop;
-        msg.hdop = data.hdop;
-        msg.vdop = data.vdop;
-        msg.gnss_id = data.gnss_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaGnssant(const fpa::FpaGnssantPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::FpaGnssant msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.gnss1_state = FpaAntStateToMsg(msg, payload.gnss1_state);
+        msg.gnss1_power = FpaAntPowerToMsg(msg, payload.gnss1_power);
+        msg.gnss1_age = (payload.gnss1_age.valid ? payload.gnss1_age.value : -1);
+        msg.gnss2_state = FpaAntStateToMsg(msg, payload.gnss2_state);
+        msg.gnss2_power = FpaAntPowerToMsg(msg, payload.gnss2_power);
+        msg.gnss2_age = (payload.gnss2_age.valid ? payload.gnss2_age.value : -1);
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_GST& data, ros::Publisher& pub) {
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishFpaGnsscorr(const fpa::FpaGnsscorrPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gpgst msg;
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.rms_range = data.rms_range;
-        msg.std_major = data.std_major;
-        msg.std_minor = data.std_minor;
-        msg.angle_major = data.angle_major;
-        msg.std_lat = data.std_lat;
-        msg.std_lon = data.std_lon;
-        msg.std_alt = data.std_alt;
-        
-        // Publish message
+        fixposition_driver_msgs::FpaGnsscorr msg;
+        msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.gnss1_fix = FpaGnssFixToMsg(msg, payload.gnss1_fix);
+        msg.gnss1_nsig_l1 = (payload.gnss1_nsig_l1.valid ? payload.gnss1_nsig_l1.value : -1);
+        msg.gnss1_nsig_l2 = (payload.gnss1_nsig_l2.valid ? payload.gnss1_nsig_l2.value : -1);
+        msg.gnss2_fix = FpaGnssFixToMsg(msg, payload.gnss2_fix);
+        msg.gnss2_nsig_l1 = (payload.gnss2_nsig_l1.valid ? payload.gnss2_nsig_l1.value : -1);
+        msg.gnss2_nsig_l2 = (payload.gnss2_nsig_l2.valid ? payload.gnss2_nsig_l2.value : -1);
+        msg.corr_latency = (payload.corr_latency.valid ? payload.corr_latency.value : 0.0f);
+        msg.corr_update_rate = (payload.corr_update_rate.valid ? payload.corr_update_rate.value : 0.0f);
+        msg.corr_data_rate = (payload.corr_data_rate.valid ? payload.corr_data_rate.value : 0.0f);
+        msg.corr_msg_rate = (payload.corr_msg_rate.valid ? payload.corr_msg_rate.value : 0.0f);
+        msg.sta_id = (payload.sta_id.valid ? payload.sta_id.value : -1);
+        FpaFloat3ToVector3(payload.sta_llh, msg.sta_llh);
+        msg.sta_dist = (payload.sta_dist.valid ? payload.sta_dist.value : -1);
         pub.publish(msg);
     }
 }
+// ---------------------------------------------------------------------------------------------------------------------
 
-void FpToRosMsg(const GX_GSV& data, ros::Publisher& pub) {
+void PublishFpaTp(const fpa::FpaTpPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gxgsv msg;
-
-        // Populate message
-        msg.sentences = data.sentences;
-        msg.sent_num = data.sent_num;
-        msg.num_sats = data.num_sats;
-        
-        for (unsigned int i = 0; i < data.sat_id.size(); i++) {
-            msg.sat_id.push_back(data.sat_id.at(i));
-            msg.elev.push_back(data.elev.at(i));
-            msg.azim.push_back(data.azim.at(i));
-            msg.cno.push_back(data.cno.at(i));
-        }
-
-        msg.signal_id = data.signal_id;
-
-        // Publish message
+        fixposition_driver_msgs::FpaTp msg;
+        msg.tp_name = payload.tp_name;
+        msg.timebase = FpaTimebaseToMsg(msg, payload.timebase);
+        msg.timeref = FpaTimerefToMsg(msg, payload.timeref);
+        msg.tp_week = payload.tp_week.value;
+        msg.tp_tow_sec = payload.tp_tow_sec.value;
+        msg.tp_tow_psec = payload.tp_tow_psec.value;
+        msg.gps_leaps = payload.gps_leaps.value;
         pub.publish(msg);
     }
 }
 
+// ---------------------------------------------------------------------------------------------------------------------
 
-void FpToRosMsg(const GP_HDT& data, ros::Publisher& pub) {
+void PublishFpaText(const fpa::FpaTextPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gphdt msg;
-
-        // Populate message
-        msg.heading = data.heading;
-        msg.true_ind = data.true_ind;
-
-        // Publish message
+        fixposition_driver_msgs::FpaText msg;
+        msg.level = FpaTextLevelToMsg(msg, payload.level);
+        msg.text = payload.text;
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_RMC& data, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gprmc msg;
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.status = data.status;
-        msg.latitude = data.latlon(0);
-        msg.lat_ns = data.lat_ns;
-        msg.longitude = data.latlon(1);
-        msg.lon_ew = data.lon_ew;
-        msg.speed = data.speed;
-        msg.course = data.course;
-        msg.date = data.date_str;
-        msg.mode = data.mode;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
-        pub.publish(msg);
+template <typename SomeFpaImuPayload>
+static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::Imu msg) {
+    msg.header.stamp = ros1::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+    msg.header.frame_id = IMU_FRAME_ID;
+    if (payload.acc.valid) {
+        msg.linear_acceleration.x = payload.acc.values[0];
+        msg.linear_acceleration.y = payload.acc.values[1];
+        msg.linear_acceleration.z = payload.acc.values[2];
+    }
+    if (payload.rot.valid) {
+        msg.angular_velocity.x = payload.rot.values[0];
+        msg.angular_velocity.y = payload.rot.values[1];
+        msg.angular_velocity.z = payload.rot.values[2];
     }
 }
 
-void FpToRosMsg(const GP_VTG& data, ros::Publisher& pub) {
+void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gpvtg msg;
-
-        // Populate message
-        msg.cog_true = data.cog_true;
-        msg.cog_ref_t = data.cog_ref_t;
-        msg.cog_mag = data.cog_mag;
-        msg.cog_ref_m = data.cog_ref_m;
-        msg.sog_knot = data.sog_knot;
-        msg.sog_unit_n = data.sog_unit_n;
-        msg.sog_kph = data.sog_kph;
-        msg.sog_unit_k = data.sog_unit_k;
-        msg.mode = data.mode;
-
-        // Publish message
+        sensor_msgs::Imu msg;
+        FpaImuPayloadToRos(payload, msg);
         pub.publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_ZDA& data, ros::Publisher& pub) {
+void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::gpzda msg;
-
-        // ROS Header
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-        }
-        msg.header.frame_id = "FP_POI";
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.date = data.date_str;
-        msg.local_hr = data.local_hr;
-        msg.local_min = data.local_min;
-
-        // Publish message
+        sensor_msgs::Imu msg;
+        FpaImuPayloadToRos(payload, msg);
         pub.publish(msg);
     }
 }
 
-void TfDataToMsg(const TfData& data, geometry_msgs::TransformStamped& msg) {
-    msg.header.frame_id = data.frame_id;
-    msg.child_frame_id = data.child_frame_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-    if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-        msg.header.stamp = ros::Time::now();
-    } else {
-        msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
+bool PublishNovbBestgnsspos(const novb::NovbHeader* header, const novb::NovbBestgnsspos* payload, ros::Publisher& pub1,
+                            ros::Publisher& pub2) {
+    if ((header == NULL) || (payload == NULL) || !header->IsLongHeader() ||
+        !((header->Source() == novb::NovbMsgTypeSource::PRIMARY) ||
+          (header->Source() == novb::NovbMsgTypeSource::SECONDARY))) {
+        return false;
     }
+    ros::Publisher pub = (header->Source() == novb::NovbMsgTypeSource::PRIMARY ? pub1 : pub2);
+    if (pub.getNumSubscribers() > 0) {
+        sensor_msgs::NavSatFix msg;
 
-    tf::quaternionEigenToMsg(data.rotation, msg.transform.rotation);
-    tf::vectorEigenToMsg(data.translation, msg.transform.translation);
-}
+        time::Time stamp;
+        if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3,
+                             time::WnoTow::Sys::GPS})) {
+            msg.header.stamp = ros1::utils::ConvTime(stamp);
+        }
 
-void NavSatFixDataToMsg(const NavSatFixData& data, sensor_msgs::NavSatFix& msg) {
-    if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-        msg.header.stamp = ros::Time::now();
-    } else {
-        msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
-    }
+        msg.header.frame_id = (header->Source() == novb::NovbMsgTypeSource::PRIMARY ? GNSS1_FRAME_ID : GNSS2_FRAME_ID);
+        msg.status.status =
+            NovbPosOrVelTypeToNavSatStatusStatus(msg.status, static_cast<novb::NovbPosOrVelType>(payload->pos_type));
+        if (msg.status.status != msg.status.STATUS_NO_FIX) {
+            msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS |
+                                  msg.status.SERVICE_GALILEO);
+        }
 
-    msg.header.frame_id = data.frame_id;
-    msg.status.status = data.status.status;
-    msg.status.service = data.status.service;
-    msg.latitude = data.latitude;
-    msg.longitude = data.longitude;
-    msg.altitude = data.altitude;
-
-    Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
-        Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
-    cov_map = data.cov;
+        msg.latitude = payload->lat;
+        msg.longitude = payload->lon;
+        msg.altitude = payload->hgt;
 
-    msg.position_covariance_type = data.position_covariance_type;
-}
+        msg.position_covariance_type = msg.COVARIANCE_TYPE_DIAGONAL_KNOWN;
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
+            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
 
-void PoseWithCovDataToMsg(const PoseWithCovData& data, geometry_msgs::PoseWithCovariance& msg) {
-    tf::pointEigenToMsg(data.position, msg.pose.position);
-    tf::quaternionEigenToMsg(data.orientation, msg.pose.orientation);
+        Eigen::Array3d cov_diag(payload->lat_stdev, payload->lon_stdev, payload->hgt_stdev);
+        cov_map = (cov_diag * cov_diag).matrix().asDiagonal();
 
-    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
-    cov_map = data.cov;
+        pub.publish(msg);
+    }
+    return true;
 }
 
-void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::TwistWithCovariance& msg) {
-    tf::vectorEigenToMsg(data.linear, msg.twist.linear);
-    tf::vectorEigenToMsg(data.angular, msg.twist.angular);
-
-    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
-    cov_map = data.cov;
-}
+// ---------------------------------------------------------------------------------------------------------------------
 
-void OdometryDataToTf(const FP_ODOMETRY& data, tf2_ros::TransformBroadcaster& pub) {
-    if (data.fusion_status > 0) {
-        // Ensure Fusion orientation is a valid quaternion
-        if (data.odom.pose.orientation.vec().isZero() && (data.odom.pose.orientation.w() == 0)) {
-            return;
+void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaGga msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
         }
-        
-        // Populate message
-        geometry_msgs::TransformStamped msg;
-        OdomToTf(data.odom, msg);
-
-        // Publish message
-        pub.sendTransform(msg);
+        msg.latitude = (payload.llh.latlon_valid ? payload.llh.lat : NAN);
+        msg.longitude = (payload.llh.latlon_valid ? payload.llh.lon : NAN);
+        msg.height = (payload.llh.height_valid ? payload.llh.height : NAN);
+        msg.quality = NmeaQualityGgaToMsg(msg, payload.quality);
+        msg.num_sv = (payload.num_sv.valid ? payload.num_sv.value : -1);
+        msg.hdop = (payload.hdop.valid ? payload.hdop.value : NAN);
+        msg.diff_age = (payload.diff_age.valid ? payload.diff_age.value : NAN);
+        msg.diff_sta = payload.diff_sta.value;
+        pub.publish(msg);
     }
 }
 
-void OdomToTf(const OdometryData& data, geometry_msgs::TransformStamped& tf) {
-    // Populate message
-    tf.header.frame_id = data.frame_id;
-    tf.child_frame_id = data.child_frame_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-    if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-        tf.header.stamp = ros::Time::now();
-    } else {
-        tf.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
+void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaGll msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.latitude = (payload.ll.latlon_valid ? payload.ll.lat : NAN);
+        msg.longitude = (payload.ll.latlon_valid ? payload.ll.lon : NAN);
+        msg.status = NmeaStatusGllRmcToMsg(msg, payload.status);
+        msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode);
+        pub.publish(msg);
     }
-
-    tf::quaternionEigenToMsg(data.pose.orientation, tf.transform.rotation);
-    tf::vectorEigenToMsg(data.pose.position, tf.transform.translation);
 }
 
-void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::TransformStamped>>& tf_map, tf2_ros::StaticTransformBroadcaster& static_br_, tf2_ros::TransformBroadcaster& br_) {
-    if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) {
-        // Publish FP_ECEF -> map
-        tf_map.at("ECEFENU0")->child_frame_id = "map";
-        static_br_.sendTransform(*tf_map.at("ECEFENU0"));
-
-        // Compute FP_ENU0 -> FP_POISH
-        // Extract translation and rotation from ECEFENU0
-        geometry_msgs::Vector3 trans_ecef_enu0 = tf_map.at("ECEFENU0")->transform.translation;
-        geometry_msgs::Quaternion rot_ecef_enu0 = tf_map.at("ECEFENU0")->transform.rotation;
-        Eigen::Vector3d t_ecef_enu0_;
-        t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z;
-        Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z);
-
-        // Extract translation and rotation from ECEFPOISH
-        geometry_msgs::Vector3 trans_ecef_poish = tf_map.at("ECEFPOISH")->transform.translation;
-        geometry_msgs::Quaternion rot_ecef_poish = tf_map.at("ECEFPOISH")->transform.rotation;
-        Eigen::Vector3d t_ecef_poish;
-        t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z;
-        Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Compute the ENU transformation
-        const Eigen::Vector3d t_enu0_poish = fixposition::TfEnuEcef(t_ecef_poish, fixposition::TfWgs84LlhEcef(t_ecef_enu0_));
-        const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish;
-
-        // Create tf2::Transform tf_ENU0POISH
-        tf2::Transform tf_ENU0POISH;
-        tf_ENU0POISH.setOrigin(tf2::Vector3(t_enu0_poish.x(), t_enu0_poish.y(), t_enu0_poish.z()));
-        tf2::Quaternion tf_q_enu0_poish(q_enu0_poish.x(), q_enu0_poish.y(), q_enu0_poish.z(), q_enu0_poish.w());
-        tf_ENU0POISH.setRotation(tf_q_enu0_poish);
-
-        // Publish map -> odom
-        // Multiply the transforms
-        tf2::Transform tf_ENU0POI;
-        tf2::fromMsg(tf_map.at("ENU0POI")->transform, tf_ENU0POI);
-        tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse();
-
-        // Create a new TransformStamped message
-        geometry_msgs::TransformStamped tf_map_odom;
-        tf_map_odom.header.stamp = ros::Time::now();
-        tf_map_odom.header.frame_id = "map";
-        tf_map_odom.child_frame_id = "odom";
-        tf_map_odom.transform = tf2::toMsg(tf_combined);
-        br_.sendTransform(tf_map_odom);
+void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaGsa msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.system = NmeaSystemIdToMsg(msg, payload.system);
+        msg.opmode = NmeaOpModeGsaToMsg(msg, payload.opmode);
+        msg.navmode = NmeaNavModeGsaToMsg(msg, payload.navmode);
+        for (auto& sat : payload.sats) {
+            if (sat.valid) {
+                msg.sats.push_back(sat.svid);
+            }
+        }
+        msg.pdop = (payload.pdop.valid ? payload.pdop.value : NAN);
+        msg.hdop = (payload.hdop.valid ? payload.hdop.value : NAN);
+        msg.vdop = (payload.vdop.valid ? payload.vdop.value : NAN);
+        pub.publish(msg);
+    }
+}
 
-        // Publish odom -> base_link
-        geometry_msgs::TransformStamped tf_odom_base;
-        tf_odom_base.header.stamp = ros::Time::now();
-        tf_odom_base.header.frame_id = "odom";
-        tf_odom_base.child_frame_id = "base_link";
-        tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Send the transform
-        br_.sendTransform(tf_odom_base);
+void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaGst msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.rms_range = (payload.rms_range.valid ? payload.rms_range.value : NAN);
+        msg.std_major = (payload.std_major.valid ? payload.std_major.value : NAN);
+        msg.std_minor = (payload.std_minor.valid ? payload.std_minor.value : NAN);
+        msg.angle_major = (payload.angle_major.valid ? payload.angle_major.value : NAN);
+        msg.std_lat = (payload.std_lat.valid ? payload.std_lat.value : NAN);
+        msg.std_lon = (payload.std_lon.valid ? payload.std_lon.value : NAN);
+        msg.std_alt = (payload.std_alt.valid ? payload.std_alt.value : NAN);
+        pub.publish(msg);
     }
 }
+// ---------------------------------------------------------------------------------------------------------------------
 
-void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) {
+void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        sensor_msgs::NavSatFix msg;
-        
-        // Populate message header
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp));
+        fixposition_driver_msgs::NmeaGsv msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.system = NmeaSystemIdToMsg(msg, payload.system);
+        msg.signal = NmeaSignalIdToMsg(msg, payload.signal);
+        msg.num_msgs = payload.num_msgs.value;
+        msg.msg_num = payload.msg_num.value;
+        for (auto& azel : payload.azels) {
+            if (azel.valid) {
+                msg.azel_sat.push_back(azel.svid);
+                msg.az.push_back(azel.az);
+                msg.el.push_back(azel.el);
+            }
         }
-        msg.header.frame_id = data.odom.child_frame_id;
-        
-        // Populate LLH position
-        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
-            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
-
-        if (data.odom.pose.position.isZero()) {
-            msg.latitude  = 0;
-            msg.longitude = 0;
-            msg.altitude  = 0;
-            msg.position_covariance_type = 0;
-            cov_map = Eigen::Matrix3d::Zero();
-        } else {
-            const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position);
-            msg.latitude  = RadToDeg(llh_pos(0));
-            msg.longitude = RadToDeg(llh_pos(1));
-            msg.altitude  = llh_pos(2);
-
-            // Populate LLH covariance
-            const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3);
-            const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position);
-            const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose();
-            cov_map = p_cov_l;
-            msg.position_covariance_type = 3;
+        for (auto& cno : payload.cnos) {
+            if (cno.valid) {
+                msg.cno_sat.push_back(cno.svid);
+                msg.cno.push_back(cno.cno);
+            }
         }
+        pub.publish(msg);
+    }
+}
 
-        // Populate LLH status
-        int status_flag = std::max(data.gnss1_status, data.gnss2_status);
-
-        if (status_flag < static_cast<int8_t>(GnssStatus::FIX_TYPE_S2D)) {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_NO_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_NONE);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        } else if (status_flag >= static_cast<int8_t>(GnssStatus::FIX_TYPE_S2D) || status_flag < static_cast<int8_t>(GnssStatus::FIX_TYPE_RTK_FLOAT)) {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_ALL);
+void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaHdt msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.heading = (payload.heading.valid ? payload.heading.value : NAN);
+        pub.publish(msg);
+    }
+}
 
-        } else if (status_flag >= static_cast<int8_t>(GnssStatus::FIX_TYPE_RTK_FLOAT)) {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_GBAS_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_ALL);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        } else {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_NO_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_NONE);
+void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaRmc msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.date.valid) {
+            msg.date_valid = true;
+            msg.date_y = payload.date.years;
+            msg.date_m = payload.date.months;
+            msg.date_d = payload.date.days;
         }
-
-        // Publish message
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.status = NmeaStatusGllRmcToMsg(msg, payload.status);
+        msg.mode = NmeaModeRmcGnsToMsg(msg, payload.mode);
+        msg.navstatus = NmeaNavStatusRmcToMsg(msg, payload.navstatus);
+        msg.latitude = (payload.llh.latlon_valid ? payload.llh.lat : NAN);
+        msg.longitude = (payload.llh.latlon_valid ? payload.llh.lon : NAN);
+        msg.height = (payload.llh.height_valid ? payload.llh.height : NAN);
+        msg.speed = (payload.speed.valid ? payload.speed.value : NAN);
+        msg.course = (payload.course.valid ? payload.course.value : NAN);
         pub.publish(msg);
     }
 }
 
-void OdomToImuMsg(const FP_ODOMETRY& data, ros::Publisher& pub) {
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        sensor_msgs::Imu msg;
+        fixposition_driver_msgs::NmeaVtg msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.cogt = (payload.cogt.valid ? payload.cogt.value : NAN);
+        msg.cogm = (payload.cogm.valid ? payload.cogm.value : NAN);
+        msg.sogn = (payload.sogn.valid ? payload.sogn.value : NAN);
+        msg.sogk = (payload.sogk.valid ? payload.sogk.value : NAN);
+        msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode);
+        pub.publish(msg);
+    }
+}
 
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp));
-        }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        tf::vectorEigenToMsg(data.acceleration, msg.linear_acceleration);
-        tf::vectorEigenToMsg(data.odom.twist.angular, msg.angular_velocity);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaZda msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.date.valid) {
+            msg.date_valid = true;
+            msg.date_y = payload.date.years;
+            msg.date_m = payload.date.months;
+            msg.date_d = payload.date.days;
+        }
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.local_hr = payload.local_hr.value;
+        msg.local_min = payload.local_min.value;
         pub.publish(msg);
     }
 }
 
-void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub) {
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg, ros::Publisher& pub) {
     if (pub.getNumSubscribers() > 0) {
-        // Create message
-        geometry_msgs::Vector3Stamped msg;
+        fixposition_driver_msgs::ParserMsg ros_msg;
+        ros_msg.header.stamp = ros::Time::now();
+        ros_msg.protocol = ParserProtocolToMsg(ros_msg, msg.proto_);
+        ros_msg.data = msg.data_;
+        ros_msg.name = msg.name_;
+        ros_msg.seq = msg.seq_;
+        msg.MakeInfo();
+        ros_msg.info = msg.info_;
+        pub.publish(ros_msg);
+    }
+}
 
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(times::GpsTimeToPtime(data.stamp));
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishNmeaEpochData(const NmeaEpochData& data, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::NmeaEpoch msg;
+        msg.header.stamp = ros1::utils::ConvTime(data.stamp_);
+        msg.header.frame_id = data.frame_id_;
+        if (data.date_.valid) {
+            msg.date_valid = true;
+            msg.date_y = data.date_.years;
+            msg.date_m = data.date_.months;
+            msg.date_d = data.date_.days;
+        }
+        if (data.time_.valid) {
+            msg.time_valid = true;
+            msg.time_h = data.time_.hours;
+            msg.time_m = data.time_.mins;
+            msg.time_s = data.time_.secs;
+        }
+        msg.status = NmeaStatusGllRmcToMsg(msg, data.status_);
+        msg.navstatus = NmeaNavStatusRmcToMsg(msg, data.navstatus_);
+        msg.mode1 = NmeaModeRmcGnsToMsg(msg, data.mode1_);
+        msg.mode2 = NmeaModeGllVtgToMsg(msg, data.mode2_);
+        msg.quality = NmeaQualityGgaToMsg(msg, data.quality_);
+        msg.opmode = NmeaOpModeGsaToMsg(msg, data.opmode_);
+        msg.navmode = NmeaNavModeGsaToMsg(msg, data.navmode_);
+        msg.latitude = (data.llh_.latlon_valid ? data.llh_.lat : NAN);
+        msg.longitude = (data.llh_.latlon_valid ? data.llh_.lon : NAN);
+        msg.height = (data.llh_.height_valid ? data.llh_.height : NAN);
+        msg.num_sv = (data.num_sv_.valid ? data.num_sv_.value : -1);
+        msg.rms_range = (data.rms_range_.valid ? data.rms_range_.value : NAN);
+        msg.std_major = (data.std_major_.valid ? data.std_major_.value : NAN);
+        msg.std_minor = (data.std_minor_.valid ? data.std_minor_.value : NAN);
+        msg.angle_major = (data.angle_major_.valid ? data.angle_major_.value : NAN);
+        msg.std_lat = (data.std_lat_.valid ? data.std_lat_.value : NAN);
+        msg.std_lon = (data.std_lon_.valid ? data.std_lon_.value : NAN);
+        msg.std_alt = (data.std_alt_.valid ? data.std_alt_.value : NAN);
+        msg.pdop = (data.pdop_.valid ? data.pdop_.value : NAN);
+        msg.hdop = (data.hdop_.valid ? data.hdop_.value : NAN);
+        msg.vdop = (data.vdop_.valid ? data.vdop_.value : NAN);
+        msg.heading = (data.heading_.valid ? data.heading_.value : NAN);
+        msg.speed = (data.speed_.valid ? data.speed_.value : NAN);
+        msg.course = (data.course_.valid ? data.course_.value : NAN);
+        msg.cogt = (data.cogt_.valid ? data.cogt_.value : NAN);
+        msg.cogm = (data.cogm_.valid ? data.cogm_.value : NAN);
+        msg.sogn = (data.sogn_.valid ? data.sogn_.value : NAN);
+        msg.sogk = (data.sogk_.valid ? data.sogk_.value : NAN);
+        msg.diff_age = (data.diff_age_.valid ? data.diff_age_.value : NAN);
+        msg.diff_sta = data.diff_sta_.value;
+        for (auto& sat : data.gsa_gsv_.sats_) {
+            fixposition_driver_msgs::NmeaSatellite sat_msg;
+            sat_msg.system = NmeaSystemIdToMsg(sat_msg, sat.system_);
+            sat_msg.svid = sat.svid_;
+            sat_msg.az = sat.az_;
+            sat_msg.el = sat.el_;
+            msg.sats.push_back(sat_msg);
+        }
+        for (auto& sig : data.gsa_gsv_.sigs_) {
+            fixposition_driver_msgs::NmeaSignal sig_msg;
+            sig_msg.system = NmeaSystemIdToMsg(sig_msg, sig.system_);
+            sig_msg.svid = sig.svid_;
+            sig_msg.signal = NmeaSignalIdToMsg(msg, sig.signal_);
+            sig_msg.cno = sig.cno_;
+            sig_msg.used = sig.used_;
+            msg.sigs.push_back(sig_msg);
         }
-        msg.header.frame_id = "FP_ENU";
 
-        // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll
-        Eigen::Vector3d enu_euler = RotToEul(data.pose.orientation.toRotationMatrix());
-        tf::vectorEigenToMsg(enu_euler, msg.vector);
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map = Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.cov_enu.data());
+        cov_map = data.cov_enu_;
 
-        // Publish message
         pub.publish(msg);
     }
 }
 
-void JumpWarningMsg(const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, const Eigen::MatrixXd& prev_cov, ros::Publisher& pub) {
-    if (pub.getNumSubscribers() > 0) {
-        // Create message
-        fixposition_driver_ros1::CovWarn msg;
-
-        // Populate message
-        if (stamp.tow == 0.0 && stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(times::GpsTimeToPtime(stamp));
-        }
+// ---------------------------------------------------------------------------------------------------------------------
 
-        std::stringstream warn_msg;
-        warn_msg << "Position jump detected! The change in position is greater than the estimated covariances. "
-                 << "Position difference: [" << pos_diff[0] << ", " << pos_diff[1] << ", " << pos_diff[2] << "], "
-                 << "Covariances: [" << prev_cov(0,0) << ", " << prev_cov(1,1) << ", " << prev_cov(2,2) << "]";
+void PublishOdometryData(const OdometryData& data, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        nav_msgs::Odometry msg;
+        msg.header.stamp = ros1::utils::ConvTime(data.stamp);
+        msg.header.frame_id = data.frame_id;
+        msg.child_frame_id = data.child_frame_id;
+        PoseWithCovDataToMsg(data.pose, msg.pose);
+        TwistWithCovDataToMsg(data.twist, msg.twist);
+        pub.publish(msg);
+    }
+}
 
-        ROS_WARN("%s", warn_msg.str().c_str());
-        tf::vectorEigenToMsg(pos_diff, msg.jump);
-        tf::vectorEigenToMsg(Eigen::Vector3d(prev_cov(0,0),prev_cov(1,1),prev_cov(2,2)), msg.covariance);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishJumpWarning(const JumpDetector& jump_detector, ros::Publisher& pub) {
+    if (pub.getNumSubscribers() > 0) {
+        fixposition_driver_msgs::CovWarn msg;
+        msg.header.stamp = ros1::utils::ConvTime(jump_detector.curr_stamp_);
+        tf::vectorEigenToMsg(jump_detector.pos_diff_, msg.jump);
+        msg.covariance.x = jump_detector.prev_cov_(0, 0);
+        msg.covariance.y = jump_detector.prev_cov_(1, 1);
+        msg.covariance.z = jump_detector.prev_cov_(2, 2);
         pub.publish(msg);
     }
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
index a88ef7f..3e1623d 100644
--- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -1,470 +1,653 @@
 /**
- *  @file
- *  @brief Main function for the fixposition driver ros node
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Fixposition driver node for ROS1
  */
 
+/* LIBC/STL */
+#include <cstdlib>
+#include <cstring>
+#include <functional>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+/* EXTERNAL */
+#include <fpsdk_common/app.hpp>
+#include <fpsdk_common/logging.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/nmea.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_common/trafo.hpp>
+#include <fpsdk_common/types.hpp>
+#include <fpsdk_ros1/ext/eigen_conversions.hpp>
+#include <fpsdk_ros1/utils.hpp>
+
 /* PACKAGE */
-#include <fixposition_driver_ros1/fixposition_driver_node.hpp>
+#include "fixposition_driver_ros1/fixposition_driver_node.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& params)
-    : FixpositionDriver(params),
-      nh_("~"),
-      // FP_A messages
-      fpa_odometry_pub_(nh_.advertise<fixposition_driver_ros1::odometry>("/fixposition/fpa/odometry", 5)),
-      fpa_imubias_pub_(nh_.advertise<fixposition_driver_ros1::imubias>("/fixposition/fpa/imubias", 5)),
-      fpa_eoe_pub_(nh_.advertise<fixposition_driver_ros1::eoe>("/fixposition/fpa/eoe", 5)),
-      fpa_llh_pub_(nh_.advertise<fixposition_driver_ros1::llh>("/fixposition/fpa/llh", 5)),
-      fpa_odomenu_pub_(nh_.advertise<fixposition_driver_ros1::odomenu>("/fixposition/fpa/odomenu", 5)),
-      fpa_odomsh_pub_(nh_.advertise<fixposition_driver_ros1::odomsh>("/fixposition/fpa/odomsh", 5)),
-      fpa_odomstatus_pub_(nh_.advertise<fixposition_driver_ros1::odomstatus>("/fixposition/fpa/odomstatus", 5)),
-      fpa_gnssant_pub_(nh_.advertise<fixposition_driver_ros1::gnssant>("/fixposition/fpa/gnssant", 5)),
-      fpa_gnsscorr_pub_(nh_.advertise<fixposition_driver_ros1::gnsscorr>("/fixposition/fpa/gnsscorr", 5)),
-      fpa_text_pub_(nh_.advertise<fixposition_driver_ros1::text>("/fixposition/fpa/text", 5)),
-      fpa_tp_pub_(nh_.advertise<fixposition_driver_ros1::tp>("/fixposition/fpa/tp", 5)),
-
-      // NMEA messages
-      nmea_gpgga_pub_(nh_.advertise<fixposition_driver_ros1::gpgga>("/fixposition/nmea/gpgga", 5)),
-      nmea_gpgll_pub_(nh_.advertise<fixposition_driver_ros1::gpgll>("/fixposition/nmea/gpgll", 5)),
-      nmea_gngsa_pub_(nh_.advertise<fixposition_driver_ros1::gngsa>("/fixposition/nmea/gngsa", 5)),
-      nmea_gpgst_pub_(nh_.advertise<fixposition_driver_ros1::gpgst>("/fixposition/nmea/gpgst", 5)),
-      nmea_gxgsv_pub_(nh_.advertise<fixposition_driver_ros1::gxgsv>("/fixposition/nmea/gxgsv", 5)),
-      nmea_gphdt_pub_(nh_.advertise<fixposition_driver_ros1::gphdt>("/fixposition/nmea/gphdt", 5)),
-      nmea_gprmc_pub_(nh_.advertise<fixposition_driver_ros1::gprmc>("/fixposition/nmea/gprmc", 5)),
-      nmea_gpvtg_pub_(nh_.advertise<fixposition_driver_ros1::gpvtg>("/fixposition/nmea/gpvtg", 5)),
-      nmea_gpzda_pub_(nh_.advertise<fixposition_driver_ros1::gpzda>("/fixposition/nmea/gpzda", 5)),
-
-      // ODOMETRY
-      odometry_ecef_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_ecef", 5)),
-      odometry_llh_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/odometry_llh", 5)),
-      odometry_enu_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_enu", 5)),
-      odometry_smooth_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_smooth", 5)),
-
-      // Orientation
-      eul_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/ypr", 5)),
-      eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/imu_ypr", 5)),
-
-      // IMU
-      rawimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/rawimu", 5)),
-      corrimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/corrimu", 5)),
-      poiimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/poiimu", 5)),
-
-      // GNSS
-      nmea_pub_(nh_.advertise<fixposition_driver_ros1::NMEA>("/fixposition/nmea", 5)),
-      navsatfix_gnss1_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss1", 5)),
-      navsatfix_gnss2_pub_(nh_.advertise<sensor_msgs::NavSatFix>("/fixposition/gnss2", 5))
-
-{
-    ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 10,
-                                                            &FixpositionDriverNode::WsCallbackRos, this,
-                                                            ros::TransportHints().tcpNoDelay());
-    rtcm_sub_ = nh_.subscribe<rtcm_msgs::Message>(params_.customer_input.rtcm_topic, 10,
-                                                  &FixpositionDriverNode::RtcmCallbackRos, this,
-                                                  ros::TransportHints().tcpNoDelay());
-
-    // Configure jump warning message
-    if (params_.fp_output.cov_warning) {
-        extras_jump_pub_ = nh_.advertise<fixposition_driver_ros1::CovWarn>("/fixposition/extras/jump", 5);
-        prev_pos.setZero();
-        prev_cov.setZero();
-    }
-    RegisterObservers();
-}
+using namespace fpsdk::common;
+using namespace fpsdk::common::parser;
 
-void FixpositionDriverNode::Run() {
-    ros::Rate rate(params_.fp_output.rate);
-
-    while (ros::ok()) {
-        // Read data and publish to ros
-        const bool connection_ok = RunOnce();
-        // process Incoming ROS msgs
-        ros::spinOnce();
-        // Handle connection loss
-        if (!connection_ok) {
-            printf("Reconnecting in %.1f seconds ...\n", params_.fp_output.reconnect_delay);
-            ros::Duration(params_.fp_output.reconnect_delay).sleep();
-            Connect();
-        } else {
-            rate.sleep();  // ensure the loop runs at the desired rate
-        }
+FixpositionDriverNode::FixpositionDriverNode(const DriverParams& driver_params, const NodeParams& node_params,
+                                             ros::NodeHandle& nh) /* clang-format off */ :
+    nh_                { nh },
+    driver_params_     { driver_params },
+    node_params_       { node_params },
+    driver_            { driver_params },
+    nmea_epoch_data_   { driver_params_.nmea_epoch_ }  // clang-format on
+{}
+
+FixpositionDriverNode::~FixpositionDriverNode() { StopNode(); }
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+// Helper for advertising output topics
+#define _PUB(_pub_, _type_, _topic_, ...)                          \
+    if (_pub_.getTopic().empty()) {                                \
+        ROS_INFO("Advertise %s (" #_type_ ")", (_topic_).c_str()); \
+        _pub_ = nh_.advertise<_type_>(_topic_, __VA_ARGS__);       \
     }
-}
 
-void FixpositionDriverNode::RegisterObservers() {
-    // NOV_B
-    bestgnsspos_obs_.push_back(std::bind(&FixpositionDriverNode::BestGnssPosToPublishNavSatFix, this,
-                                         std::placeholders::_1, std::placeholders::_2));
-
-    // FP_A
-    for (const auto& format : params_.fp_output.formats) {
-        if (format == "ODOMETRY") {
-            dynamic_cast<NmeaConverter<FP_ODOMETRY>*>(a_converters_["ODOMETRY"].get())
-                ->AddObserver([this](const FP_ODOMETRY& data) {
-                    FpToRosMsg(data, fpa_odometry_pub_);
-                    FpToRosMsg(data.odom, odometry_ecef_pub_);
-                    OdomToImuMsg(data, poiimu_pub_);
-                    OdomToNavSatFix(data, odometry_llh_pub_);
-                    OdometryDataToTf(data, br_);
-
-                    // Output jump warning
-                    if (params_.fp_output.cov_warning) {
-                        if (!prev_pos.isZero() && !prev_cov.isZero()) {
-                            Eigen::Vector3d pos_diff = (prev_pos - data.odom.pose.position).cwiseAbs();
-
-                            if ((pos_diff[0] > prev_cov(0,0)) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) {
-                                JumpWarningMsg(data.odom.stamp, pos_diff, prev_cov, extras_jump_pub_);
-                            }
-                        }
-                        prev_pos = data.odom.pose.position;
-                        prev_cov = data.odom.pose.cov;
-                    }
-                });
-        } else if (format == "ODOMENU") {
-            dynamic_cast<NmeaConverter<FP_ODOMENU>*>(a_converters_["ODOMENU"].get())
-                ->AddObserver([this](const FP_ODOMENU& data) {
-                    FpToRosMsg(data, fpa_odomenu_pub_);
-                    FpToRosMsg(data.odom, odometry_enu_pub_);
-                    OdomToYprMsg(data.odom, eul_pub_);
-
-                    // Append TF if Nav2 mode is selected
-                    if (params_.fp_output.nav2_mode) {
-                        // Get FP_ENU0 -> FP_POI
-                        geometry_msgs::TransformStamped tf;
-                        OdomToTf(data.odom, tf);
-                        tf_map["ENU0POI"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
-                    }
-                });
-        } else if (format == "ODOMSH") {
-            dynamic_cast<NmeaConverter<FP_ODOMSH>*>(a_converters_["ODOMSH"].get())
-                ->AddObserver([this](const FP_ODOMSH& data) {
-                    FpToRosMsg(data, fpa_odomsh_pub_);
-                    FpToRosMsg(data.odom, odometry_smooth_pub_);
-
-                    // Append TF if Nav2 mode is selected
-                    if (params_.fp_output.nav2_mode) {
-                        // Get FP_ECEF -> FP_POISH
-                        geometry_msgs::TransformStamped tf;
-                        OdomToTf(data.odom, tf);
-                        tf_map["ECEFPOISH"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
-                    }
-                });
-        } else if (format == "ODOMSTATUS") {
-            dynamic_cast<NmeaConverter<FP_ODOMSTATUS>*>(a_converters_["ODOMSTATUS"].get())
-                ->AddObserver([this](const FP_ODOMSTATUS& data) { FpToRosMsg(data, fpa_odomstatus_pub_); });
-        } else if (format == "IMUBIAS") {
-            dynamic_cast<NmeaConverter<FP_IMUBIAS>*>(a_converters_["IMUBIAS"].get())
-                ->AddObserver([this](const FP_IMUBIAS& data) { FpToRosMsg(data, fpa_imubias_pub_); });
-        } else if (format == "EOE") {
-            dynamic_cast<NmeaConverter<FP_EOE>*>(a_converters_["EOE"].get())
-                ->AddObserver([this](const FP_EOE& data) {
-                    FpToRosMsg(data, fpa_eoe_pub_);
-
-                    // Generate Nav2 TF tree
-                    if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) {
-                        PublishNav2Tf(tf_map, static_br_, br_);
-                    }
-                });
-        } else if (format == "LLH") {
-            dynamic_cast<NmeaConverter<FP_LLH>*>(a_converters_["LLH"].get())
-                ->AddObserver([this](const FP_LLH& data) { FpToRosMsg(data, fpa_llh_pub_); });
-        } else if (format == "GNSSANT") {
-            dynamic_cast<NmeaConverter<FP_GNSSANT>*>(a_converters_["GNSSANT"].get())
-                ->AddObserver([this](const FP_GNSSANT& data) { FpToRosMsg(data, fpa_gnssant_pub_); });
-        } else if (format == "GNSSCORR") {
-            dynamic_cast<NmeaConverter<FP_GNSSCORR>*>(a_converters_["GNSSCORR"].get())
-                ->AddObserver([this](const FP_GNSSCORR& data) { FpToRosMsg(data, fpa_gnsscorr_pub_); });
-        } else if (format == "TEXT") {
-            dynamic_cast<NmeaConverter<FP_TEXT>*>(a_converters_["TEXT"].get())
-                ->AddObserver([this](const FP_TEXT& data) { FpToRosMsg(data, fpa_text_pub_); });
-        } else if (format == "RAWIMU") {
-            dynamic_cast<NmeaConverter<FP_RAWIMU>*>(a_converters_["RAWIMU"].get())
-                ->AddObserver([this](const FP_RAWIMU& data) { FpToRosMsg(data.imu, rawimu_pub_); });
-        } else if (format == "CORRIMU") {
-            dynamic_cast<NmeaConverter<FP_CORRIMU>*>(a_converters_["CORRIMU"].get())
-                ->AddObserver([this](const FP_CORRIMU& data) { FpToRosMsg(data.imu, corrimu_pub_); });
-        } else if (format == "TF") {
-            dynamic_cast<NmeaConverter<FP_TF>*>(a_converters_["TF"].get())->AddObserver([this](const FP_TF& data) {
-                if (data.valid_tf) {
-                    // TF Observer Lambda
-                    geometry_msgs::TransformStamped tf;
-                    TfDataToMsg(data.tf, tf);
-                    if (tf.child_frame_id == "FP_IMUH" && tf.header.frame_id == "FP_POI") {
-                        br_.sendTransform(tf);
-
-                        // Publish Pitch Roll based on IMU only
-                        Eigen::Vector3d imu_ypr_eigen = QuatToEul(data.tf.rotation);
-                        imu_ypr_eigen.x() = 0.0;  // the yaw value is not observable using IMU alone
-                        geometry_msgs::Vector3Stamped imu_ypr;
-                        imu_ypr.header.stamp = tf.header.stamp;
-                        imu_ypr.header.frame_id = "FP_POI";
-                        tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr.vector);
-                        eul_imu_pub_.publish(imu_ypr);
-
-                    } else if (tf.child_frame_id == "FP_POISH" && tf.header.frame_id == "FP_POI") {
-                        br_.sendTransform(tf);
-
-                        // Append TF if Nav2 mode is selected
-                        if (params_.fp_output.nav2_mode) {
-                            // Get FP_POI -> FP_POISH
-                            tf_map["POIPOISH"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
-                        }
-                    } else if (tf.child_frame_id == "FP_ENU0" && tf.header.frame_id == "FP_ECEF") {
-                        static_br_.sendTransform(tf);
-
-                        // Append TF if Nav2 mode is selected
-                        if (params_.fp_output.nav2_mode) {
-                            // Get FP_ECEF -> FP_ENU0
-                            tf_map["ECEFENU0"] = std::make_shared<geometry_msgs::TransformStamped>(tf);
-                        }
-                    } else {
-                        static_br_.sendTransform(tf);
-                    }
-                }
-            });
-        } else if (format == "TP") {
-            dynamic_cast<NmeaConverter<FP_TP>*>(a_converters_["TP"].get())
-                ->AddObserver([this](const FP_TP& data) { FpToRosMsg(data, fpa_tp_pub_); });
-        } else if (format == "GPGGA") {
-            dynamic_cast<NmeaConverter<GP_GGA>*>(a_converters_["GPGGA"].get())->AddObserver([this](const GP_GGA& data) {
-                FpToRosMsg(data, nmea_gpgga_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) {
-                    nmea_message_.AddNmeaEpoch(data);
-                    PublishNmea();  // GPGGA controls the NMEA output
-                }
-            });
-        } else if (format == "GPGLL") {
-            dynamic_cast<NmeaConverter<GP_GLL>*>(a_converters_["GPGLL"].get())->AddObserver([this](const GP_GLL& data) {
-                FpToRosMsg(data, nmea_gpgll_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GNGSA") {
-            dynamic_cast<NmeaConverter<GN_GSA>*>(a_converters_["GNGSA"].get())->AddObserver([this](const GN_GSA& data) {
-                FpToRosMsg(data, nmea_gngsa_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPGST") {
-            dynamic_cast<NmeaConverter<GP_GST>*>(a_converters_["GPGST"].get())->AddObserver([this](const GP_GST& data) {
-                FpToRosMsg(data, nmea_gpgst_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GXGSV") {
-            dynamic_cast<NmeaConverter<GX_GSV>*>(a_converters_["GXGSV"].get())->AddObserver([this](const GX_GSV& data) {
-                FpToRosMsg(data, nmea_gxgsv_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPHDT") {
-            dynamic_cast<NmeaConverter<GP_HDT>*>(a_converters_["GPHDT"].get())->AddObserver([this](const GP_HDT& data) {
-                FpToRosMsg(data, nmea_gphdt_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPRMC") {
-            dynamic_cast<NmeaConverter<GP_RMC>*>(a_converters_["GPRMC"].get())->AddObserver([this](const GP_RMC& data) {
-                FpToRosMsg(data, nmea_gprmc_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPVTG") {
-            dynamic_cast<NmeaConverter<GP_VTG>*>(a_converters_["GPVTG"].get())->AddObserver([this](const GP_VTG& data) {
-                FpToRosMsg(data, nmea_gpvtg_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPZDA") {
-            dynamic_cast<NmeaConverter<GP_ZDA>*>(a_converters_["GPZDA"].get())->AddObserver([this](const GP_ZDA& data) {
-                FpToRosMsg(data, nmea_gpzda_pub_);
-                if (nmea_pub_.getNumSubscribers() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        }
+// Helper for subscribing to input topics
+#define _SUB(_sub_, _type_, _topic_, ...)                          \
+    do {                                                           \
+        ROS_INFO("Subscribe %s (" #_type_ ")", (_topic_).c_str()); \
+        _sub_ = nh_.subscribe<_type_>(_topic_, __VA_ARGS__);       \
+    } while (0)
+
+bool FixpositionDriverNode::StartNode() {
+    ROS_INFO("Starting...");
+
+    // Add observers and advertise output topics, depending on configuration
+    const std::string output_ns = (node_params_.output_ns_.empty() ? nh_.getNamespace() : node_params_.output_ns_);
+
+    // FP_A-ODOMETRY
+    if (driver_params_.MessageEnabled(fpa::FpaOdometryPayload::MSG_NAME)) {
+        _PUB(fpa_odometry_pub_, fixposition_driver_msgs::FpaOdometry, output_ns + "/fpa/odometry", 5);
+        _PUB(odometry_ecef_pub_, nav_msgs::Odometry, output_ns + "/odometry_ecef", 5);
+        _PUB(odometry_llh_pub_, sensor_msgs::NavSatFix, output_ns + "/odometry_llh", 5);
+        _PUB(poiimu_pub_, sensor_msgs::Imu, output_ns + "/poiimu", 5);
+        driver_.AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto odometry_payload = dynamic_cast<const fpa::FpaOdometryPayload&>(payload);
+            PublishFpaOdometry(odometry_payload, fpa_odometry_pub_);
+            PublishFpaOdometryDataImu(odometry_payload, poiimu_pub_);
+            PublishFpaOdometryDataNavSatFix(odometry_payload, odometry_llh_pub_);
+            OdometryData odometry_data;
+            odometry_data.SetFromFpaOdomPayload(odometry_payload);
+            PublishOdometryData(odometry_data, odometry_ecef_pub_);
+            ProcessOdometryData(odometry_data);
+        });
     }
-}
 
-void FixpositionDriverNode::PublishNmea() {
-    // If epoch message is complete, generate NMEA output
-    if (nmea_message_.checkEpoch()) {
-        // Generate new message
-        fixposition_driver_ros1::NMEA msg;
+    // FP_A-ODOMSH
+    if (driver_params_.MessageEnabled(fpa::FpaOdomshPayload::MSG_NAME)) {
+        _PUB(fpa_odomsh_pub_, fixposition_driver_msgs::FpaOdomsh, output_ns + "/fpa/odomsh", 5);
+        _PUB(odometry_smooth_pub_, nav_msgs::Odometry, output_ns + "/odometry_smooth", 5);
+        driver_.AddFpaObserver(fpa::FpaOdomshPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto odomsh_payload = dynamic_cast<const fpa::FpaOdomshPayload&>(payload);
+            PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_);
+            OdometryData odometry_data;
+            odometry_data.SetFromFpaOdomPayload(odomsh_payload);
+            PublishOdometryData(odometry_data, odometry_smooth_pub_);
+            ProcessOdometryData(odometry_data);
+        });
+    }
 
-        // ROS Header
-        if (nmea_message_.stamp.tow == 0.0 && nmea_message_.stamp.wno == 0) {
-            msg.header.stamp = ros::Time::now();
-        } else {
-            msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(nmea_message_.stamp));
-        }
-        msg.header.frame_id = "FP_POI";
+    // FP_A-ODOMENU
+    if (driver_params_.MessageEnabled(fpa::FpaOdomenuPayload::MSG_NAME)) {
+        _PUB(fpa_odomenu_pub_, fixposition_driver_msgs::FpaOdomenu, output_ns + "/fpa/odomenu", 5);
+        _PUB(odometry_enu_pub_, nav_msgs::Odometry, output_ns + "/odometry_enu", 5);
+        _PUB(eul_pub_, geometry_msgs::Vector3Stamped, output_ns + "/ypr", 5);
+        driver_.AddFpaObserver(fpa::FpaOdomenuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto odomenu_payload = dynamic_cast<const fpa::FpaOdomenuPayload&>(payload);
+            PublishFpaOdomenu(odomenu_payload, fpa_odomenu_pub_);
+            PublishFpaOdomenuVector3Stamped(odomenu_payload, eul_pub_);
+            OdometryData odometry_data;
+            odometry_data.SetFromFpaOdomPayload(odomenu_payload);
+            PublishOdometryData(odometry_data, odometry_enu_pub_);
+            ProcessOdometryData(odometry_data);
+        });
+    }
 
-        // Time and date fields
-        msg.time = nmea_message_.time_str;
-        msg.date = nmea_message_.date_str;
+    // FP_A-ODOMSTATUS
+    if (driver_params_.MessageEnabled(fpa::FpaOdomstatusPayload::MSG_NAME)) {
+        _PUB(fpa_odomstatus_pub_, fixposition_driver_msgs::FpaOdomstatus, output_ns + "/fpa/odomstatus", 5);
+        driver_.AddFpaObserver(fpa::FpaOdomstatusPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaOdomstatus(dynamic_cast<const fpa::FpaOdomstatusPayload&>(payload), fpa_odomstatus_pub_);
+        });
+    }
 
-        // Latitude [degrees]. Positive is north of equator; negative is south
-        msg.latitude = nmea_message_.llh(0);
+    // FP_A-EOE
+    if (driver_params_.MessageEnabled(fpa::FpaEoePayload::MSG_NAME)) {
+        _PUB(fpa_eoe_pub_, fixposition_driver_msgs::FpaEoe, output_ns + "/fpa/eoe", 5);
+        driver_.AddFpaObserver(fpa::FpaEoePayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto eoe_payload = dynamic_cast<const fpa::FpaEoePayload&>(payload);
+            PublishFpaEoe(eoe_payload, fpa_eoe_pub_);
+            // Generate Nav2 TF tree
+            if (driver_params_.nav2_mode_ && (eoe_payload.epoch == fpa::FpaEpoch::FUSION)) {
+                PublishNav2Tf();
+            }
+            // NMEA epoch
+            if (driver_params_.nmea_epoch_ == eoe_payload.epoch) {
+                PublishNmeaEpochData(nmea_epoch_data_.CompleteAndReset(), nmea_epoch_pub_);
+            }
+        });
+    }
 
-        // Longitude [degrees]. Positive is east of prime meridian; negative is west
-        msg.longitude = nmea_message_.llh(1);
+    // FP_A-TF
+    if (driver_params_.MessageEnabled(fpa::FpaTfPayload::MSG_NAME)) {
+        _PUB(eul_imu_pub_, geometry_msgs::Vector3Stamped, output_ns + "/imu_ypr", 5);
+        driver_.AddFpaObserver(fpa::FpaTfPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            TfData tf;
+            tf.SetFromFpaTfPayload(dynamic_cast<const fpa::FpaTfPayload&>(payload));
+            ProcessTfData(tf);
+        });
+    }
 
-        // Altitude [m]. Positive is above the WGS-84 ellipsoid
-        msg.altitude = nmea_message_.llh(2);
+    // FP_A-LLH
+    if (driver_params_.MessageEnabled(fpa::FpaLlhPayload::MSG_NAME)) {
+        _PUB(fpa_llh_pub_, fixposition_driver_msgs::FpaLlh, output_ns + "/fpa/llh", 5);
+        driver_.AddFpaObserver(fpa::FpaLlhPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaLlh(dynamic_cast<const fpa::FpaLlhPayload&>(payload), fpa_llh_pub_);
+        });
+    }
 
-        // Quality indicator
-        msg.quality = nmea_message_.quality;
+    // FP_A-GNSSANT
+    if (driver_params_.MessageEnabled(fpa::FpaGnssantPayload::MSG_NAME)) {
+        _PUB(fpa_gnssant_pub_, fixposition_driver_msgs::FpaGnssant, output_ns + "/fpa/gnssant", 5);
+        driver_.AddFpaObserver(fpa::FpaGnssantPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaGnssant(dynamic_cast<const fpa::FpaGnssantPayload&>(payload), fpa_gnssant_pub_);
+        });
+    }
 
-        // Number of satellites
-        msg.num_sv = nmea_message_.num_sv;
+    // FP_A-GNSSCORR
+    if (driver_params_.MessageEnabled(fpa::FpaGnsscorrPayload::MSG_NAME)) {
+        _PUB(fpa_gnsscorr_pub_, fixposition_driver_msgs::FpaGnsscorr, output_ns + "/fpa/gnsscorr", 5);
+        driver_.AddFpaObserver(fpa::FpaGnsscorrPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaGnsscorr(dynamic_cast<const fpa::FpaGnsscorrPayload&>(payload), fpa_gnsscorr_pub_);
+        });
+    }
 
-        // ID numbers of satellites used in solution
-        for (unsigned int i = 0; i < nmea_message_.ids.size(); i++) {
-            msg.ids.push_back(nmea_message_.ids.at(i));
-        }
+    // FP_A-IMUBIAS
+    if (driver_params_.MessageEnabled(fpa::FpaImubiasPayload::MSG_NAME)) {
+        _PUB(fpa_imubias_pub_, fixposition_driver_msgs::FpaImubias, output_ns + "/fpa/imubias", 5);
+        driver_.AddFpaObserver(fpa::FpaImubiasPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaImubias(dynamic_cast<const fpa::FpaImubiasPayload&>(payload), fpa_imubias_pub_);
+        });
+    }
 
-        // Dilution of precision
-        msg.hdop_rec = nmea_message_.hdop_receiver;
-        msg.pdop = nmea_message_.pdop;
-        msg.hdop = nmea_message_.hdop;
-        msg.vdop = nmea_message_.vdop;
-
-        // Populate GNSS pseudorange error statistics
-        msg.rms_range = nmea_message_.rms_range;
-        msg.std_major = nmea_message_.std_major;
-        msg.std_minor = nmea_message_.std_minor;
-        msg.angle_major = nmea_message_.angle_major;
-        msg.std_lat = nmea_message_.std_lat;
-        msg.std_lon = nmea_message_.std_lon;
-        msg.std_alt = nmea_message_.std_alt;
-
-        // Position covariance [m^2]
-        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
-            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.covariance.data());
-        cov_map = nmea_message_.cov;
-
-        // Method employed to estimate covariance
-        msg.cov_type = nmea_message_.cov_type;
-
-        // Populate GNSS satellites in view
-        for (auto gsv_it = nmea_message_.gnss_signals.begin(); gsv_it != nmea_message_.gnss_signals.end(); ++gsv_it) {
-            SignalType msg_type = gsv_it->first;
-            std::map<unsigned int, GnssSignalStats>* gnss_data = &gsv_it->second;
-
-            // Populate GnssSats message
-            fixposition_driver_ros1::GnssSats sats_msg;
-
-            // Get constellation name
-            if (msg_type == SignalType::GPS) {
-                sats_msg.constellation = "GPS";
-            } else if (msg_type == SignalType::Galileo) {
-                sats_msg.constellation = "Galileo";
-            } else if (msg_type == SignalType::BeiDou) {
-                sats_msg.constellation = "BeiDou";
-            } else if (msg_type == SignalType::GLONASS) {
-                sats_msg.constellation = "GLONASS";
-            } else {
-                sats_msg.constellation = "Unknown";
-            }
+    // FP_A-RAWIMU
+    if (driver_params_.MessageEnabled(fpa::FpaRawimuPayload::MSG_NAME)) {
+        _PUB(rawimu_pub_, sensor_msgs::Imu, output_ns + "/fpa/rawimu", 5);
+        driver_.AddFpaObserver(fpa::FpaRawimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaRawimu(dynamic_cast<const fpa::FpaRawimuPayload&>(payload), rawimu_pub_);
+        });
+    }
 
-            // Get signal statistics
-            for (auto it = gnss_data->begin(); it != gnss_data->end(); ++it) {
-                unsigned int sat_id = it->first;
-                GnssSignalStats signals = it->second;
+    // FP_A-CORRIMU
+    if (driver_params_.MessageEnabled(fpa::FpaCorrimuPayload::MSG_NAME)) {
+        _PUB(corrimu_pub_, sensor_msgs::Imu, output_ns + "/fpa/corrimu", 5);
+        driver_.AddFpaObserver(fpa::FpaCorrimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaCorrimu(dynamic_cast<const fpa::FpaCorrimuPayload&>(payload), corrimu_pub_);
+        });
+    }
 
-                sats_msg.sat_id.push_back(sat_id);
-                sats_msg.azim.push_back(signals.azim);
-                sats_msg.elev.push_back(signals.elev);
-                sats_msg.cno_l1.push_back(signals.cno_l1);
-                sats_msg.cno_l2.push_back(signals.cno_l2);
-            }
+    // FP_A-TEXT
+    if (driver_params_.MessageEnabled(fpa::FpaTextPayload::MSG_NAME)) {
+        _PUB(fpa_text_pub_, fixposition_driver_msgs::FpaText, output_ns + "/fpa/text", 5);
+        driver_.AddFpaObserver(fpa::FpaTextPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaText(dynamic_cast<const fpa::FpaTextPayload&>(payload), fpa_text_pub_);
+        });
+    }
 
-            // Add GnssSats to NMEA message
-            msg.gnss_sats.push_back(sats_msg);
-        }
+    // FP_A-TP
+    if (driver_params_.MessageEnabled(fpa::FpaTpPayload::MSG_NAME)) {
+        _PUB(fpa_tp_pub_, fixposition_driver_msgs::FpaTp, output_ns + "/fpa/tp", 5);
+        driver_.AddFpaObserver(fpa::FpaTpPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaTp(dynamic_cast<const fpa::FpaTpPayload&>(payload), fpa_tp_pub_);
+        });
+    }
 
-        // Clear map
-        nmea_message_.gnss_signals.clear();
+    // NOV_B-BESTGNSSPOS
+    if (driver_params_.MessageEnabled(novb::NOV_B_BESTGNSSPOS_STRID)) {
+        _PUB(navsatfix_gnss1_pub_, sensor_msgs::NavSatFix, output_ns + "/gnss1", 5);
+        _PUB(navsatfix_gnss2_pub_, sensor_msgs::NavSatFix, output_ns + "/gnss2", 5);
+        driver_.AddNovbObserver(  //
+            novb::NOV_B_BESTGNSSPOS_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) {
+                if (!PublishNovbBestgnsspos(header, (novb::NovbBestgnsspos*)payload, navsatfix_gnss1_pub_,
+                                            navsatfix_gnss2_pub_)) {
+                    ROS_WARN_THROTTLE(1.0, "Bad NOV_B-BESTGNSSPOS");
+                }
+            });
+    }
 
-        // True heading
-        msg.heading = nmea_message_.heading;
+    // NMEA-GP-GGA
+    if (driver_params_.MessageEnabled(nmea::NmeaGgaPayload::FORMATTER)) {
+        _PUB(nmea_gga_pub_, fixposition_driver_msgs::NmeaGga, output_ns + "/nmea/gga", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGgaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gga_payload = dynamic_cast<const nmea::NmeaGgaPayload&>(payload);
+            PublishNmeaGga(gga_payload, nmea_gga_pub_);
+            nmea_epoch_data_.gga_ = gga_payload;
+        });
+    }
 
-        // Speed over ground [m/s]
-        msg.speed = nmea_message_.speed;
+    // NMEA-GP-GLL
+    if (driver_params_.MessageEnabled(nmea::NmeaGllPayload::FORMATTER)) {
+        _PUB(nmea_gll_pub_, fixposition_driver_msgs::NmeaGll, output_ns + "/nmea/gll", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGllPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gll_payload = dynamic_cast<const nmea::NmeaGllPayload&>(payload);
+            PublishNmeaGll(gll_payload, nmea_gll_pub_);
+            nmea_epoch_data_.gll_ = gll_payload;
+        });
+    }
 
-        // Course over ground [deg]
-        msg.course = nmea_message_.course;
+    // NMEA-GN-GSA
+    if (driver_params_.MessageEnabled(nmea::NmeaGsaPayload::FORMATTER)) {
+        _PUB(nmea_gsa_pub_, fixposition_driver_msgs::NmeaGsa, output_ns + "/nmea/gsa", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGsaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gsa_payload_ = dynamic_cast<const nmea::NmeaGsaPayload&>(payload);
+            PublishNmeaGsa(gsa_payload_, nmea_gsa_pub_);
+            nmea_epoch_data_.gsa_ = gsa_payload_;
+            nmea_epoch_data_.gsa_gsv_.AddGsa(gsa_payload_);
+        });
+    }
 
-        // Populate differential data information
-        msg.diff_age = nmea_message_.diff_age;
-        msg.diff_sta = nmea_message_.diff_sta;
+    // NMEA-GP-GST
+    if (driver_params_.MessageEnabled(nmea::NmeaGstPayload::FORMATTER)) {
+        _PUB(nmea_gst_pub_, fixposition_driver_msgs::NmeaGst, output_ns + "/nmea/gst", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGstPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gst_payload = dynamic_cast<const nmea::NmeaGstPayload&>(payload);
+            PublishNmeaGst(gst_payload, nmea_gst_pub_);
+            nmea_epoch_data_.gst_ = gst_payload;
+        });
+    }
 
-        // Publish message
-        nmea_pub_.publish(msg);
+    // NMEA-GX-GSV
+    if (driver_params_.MessageEnabled(nmea::NmeaGsvPayload::FORMATTER)) {
+        _PUB(nmea_gsv_pub_, fixposition_driver_msgs::NmeaGsv, output_ns + "/nmea/gsv", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGsvPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gsv_payload_ = dynamic_cast<const nmea::NmeaGsvPayload&>(payload);
+            PublishNmeaGsv(gsv_payload_, nmea_gsv_pub_);
+            nmea_epoch_data_.gsa_gsv_.AddGsv(gsv_payload_);
+        });
     }
-}
 
-void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros1::SpeedConstPtr& msg) {
-    std::unordered_map<std::string, std::vector<std::pair<bool, int>>> measurements;
-    for (const auto& sensor : msg->sensors) {
-        measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx});
-        measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy});
-        measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz});
+    // NMEA-GP-HDT
+    if (driver_params_.MessageEnabled(nmea::NmeaHdtPayload::FORMATTER)) {
+        _PUB(nmea_hdt_pub_, fixposition_driver_msgs::NmeaHdt, output_ns + "/nmea/hdt", 5);
+        driver_.AddNmeaObserver(nmea::NmeaHdtPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto hdt_payload = dynamic_cast<const nmea::NmeaHdtPayload&>(payload);
+            PublishNmeaHdt(hdt_payload, nmea_hdt_pub_);
+            nmea_epoch_data_.hdt_ = hdt_payload;
+        });
     }
-    WsCallback(measurements);
+
+    // NMEA-GP-RMC
+    if (driver_params_.MessageEnabled(nmea::NmeaRmcPayload::FORMATTER)) {
+        _PUB(nmea_rmc_pub_, fixposition_driver_msgs::NmeaRmc, output_ns + "/nmea/rmc", 5);
+        driver_.AddNmeaObserver(nmea::NmeaRmcPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto rmc_payload = dynamic_cast<const nmea::NmeaRmcPayload&>(payload);
+            PublishNmeaRmc(rmc_payload, nmea_rmc_pub_);
+            nmea_epoch_data_.rmc_ = rmc_payload;
+        });
+    }
+
+    // NMEA-GP-VTG
+    if (driver_params_.MessageEnabled(nmea::NmeaVtgPayload::FORMATTER)) {
+        _PUB(nmea_vtg_pub_, fixposition_driver_msgs::NmeaVtg, output_ns + "/nmea/vtg", 5);
+        driver_.AddNmeaObserver(nmea::NmeaVtgPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto vtg_payload = dynamic_cast<const nmea::NmeaVtgPayload&>(payload);
+            PublishNmeaVtg(vtg_payload, nmea_vtg_pub_);
+            nmea_epoch_data_.vtg_ = vtg_payload;
+        });
+    }
+
+    // NMEA-GP-ZDA
+    if (driver_params_.MessageEnabled(nmea::NmeaZdaPayload::FORMATTER)) {
+        _PUB(nmea_zda_pub_, fixposition_driver_msgs::NmeaZda, output_ns + "/nmea/zda", 5);
+        driver_.AddNmeaObserver(nmea::NmeaZdaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto zda_payload = dynamic_cast<const nmea::NmeaZdaPayload&>(payload);
+            PublishNmeaZda(zda_payload, nmea_zda_pub_);
+            nmea_epoch_data_.zda_ = zda_payload;
+        });
+    }
+
+    // Raw messages
+    if (driver_params_.raw_output_) {
+        _PUB(raw_pub_, fixposition_driver_msgs::ParserMsg, output_ns + "/raw", 5);
+        driver_.AddRawObserver([this](const parser::ParserMsg& msg) { PublishParserMsg(msg, raw_pub_); });
+    }
+
+    // NMEA epoch
+    if (driver_params_.nmea_epoch_ != fpa::FpaEpoch::UNSPECIFIED) {
+        _PUB(nmea_epoch_pub_, fixposition_driver_msgs::NmeaEpoch, output_ns + "/nmea", 5);
+        // Publish is triggered by FP_A-EOE above
+    }
+
+    // Jump warning message
+    if (driver_params_.cov_warning_) {
+        _PUB(jump_pub_, fixposition_driver_msgs::CovWarn, output_ns + "/extras/jump", 5);
+    }
+
+    // Subscribe to correction data input
+    if (!node_params_.corr_topic_.empty()) {
+        _SUB(corr_sub_, rtcm_msgs::Message, node_params_.corr_topic_, 100,
+             [this](const rtcm_msgs::MessageConstPtr& msg) {
+                 driver_.SendCorrectionData(msg->message.data(), msg->message.size());
+             });
+    }
+
+    // Subscribe to wheelspeed input
+    if (!node_params_.speed_topic_.empty()) {
+        _SUB(ws_sub_, fixposition_driver_msgs::Speed, node_params_.speed_topic_, 10,
+             [this](const fixposition_driver_msgs::SpeedConstPtr& msg) {
+                 driver_.SendWheelspeedData(SpeedMsgToWheelspeedData(*msg));
+             });
+    }
+
+    return driver_.StartDriver();
 }
 
-void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::MessageConstPtr& msg) {
-    const void* rtcm_msg = &(msg->message[0]);
-    size_t msg_size = msg->message.size();
-    RtcmCallback(rtcm_msg, msg_size);
+#undef _PUB
+#undef _SUB
+
+void FixpositionDriverNode::StopNode() {
+    ROS_INFO("Stopping...");
+
+    driver_.RemoveFpaObservers();
+    driver_.RemoveNmeaObservers();
+    driver_.RemoveNovbObservers();
+
+    driver_.StopDriver();
+
+    // Stop advertising output topics
+    // - FP_A messages
+    fpa_eoe_pub_.shutdown();
+    fpa_gnssant_pub_.shutdown();
+    fpa_gnsscorr_pub_.shutdown();
+    fpa_imubias_pub_.shutdown();
+    fpa_llh_pub_.shutdown();
+    fpa_odomenu_pub_.shutdown();
+    fpa_odometry_pub_.shutdown();
+    fpa_odomsh_pub_.shutdown();
+    fpa_odomstatus_pub_.shutdown();
+    fpa_text_pub_.shutdown();
+    fpa_tp_pub_.shutdown();
+    // - NMEA messages
+    nmea_gga_pub_.shutdown();
+    nmea_gll_pub_.shutdown();
+    nmea_gsa_pub_.shutdown();
+    nmea_gst_pub_.shutdown();
+    nmea_gsv_pub_.shutdown();
+    nmea_hdt_pub_.shutdown();
+    nmea_rmc_pub_.shutdown();
+    nmea_vtg_pub_.shutdown();
+    nmea_zda_pub_.shutdown();
+    // - Odometry
+    odometry_ecef_pub_.shutdown();
+    odometry_enu_pub_.shutdown();
+    odometry_llh_pub_.shutdown();
+    odometry_smooth_pub_.shutdown();
+    // - Orientation
+    eul_pub_.shutdown();
+    eul_imu_pub_.shutdown();
+    // - IMU
+    corrimu_pub_.shutdown();
+    poiimu_pub_.shutdown();
+    rawimu_pub_.shutdown();
+    // - GNSS
+    navsatfix_gnss1_pub_.shutdown();
+    navsatfix_gnss2_pub_.shutdown();
+    nmea_epoch_pub_.shutdown();
+    // - Other
+    jump_pub_.shutdown();
+    raw_pub_.shutdown();
+
+    // Stop input message subscribers
+    ws_sub_.shutdown();
+    corr_sub_.shutdown();
 }
 
-void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header,
-                                                          const BESTGNSSPOSMem* payload) {
-    // Buffer to data struct
-    NavSatFixData nav_sat_fix;
-    NovToData(header, payload, nav_sat_fix);
-
-    // Publish
-    if (nav_sat_fix.frame_id == "GNSS1" || nav_sat_fix.frame_id == "GNSS") {
-        if (navsatfix_gnss1_pub_.getNumSubscribers() > 0) {
-            sensor_msgs::NavSatFix msg;
-            NavSatFixDataToMsg(nav_sat_fix, msg);
-            navsatfix_gnss1_pub_.publish(msg);
+// ---------------------------------------------------------------------------------------------------------------------
+
+void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) {
+    geometry_msgs::TransformStamped tf;
+    TfDataToTransformStamped(tf_data, tf);
+
+    // TODO: use constants from helper.hpp?
+
+    if ((tf.child_frame_id == "FP_IMUH") && (tf.header.frame_id == "FP_POI")) {
+        tf_br_.sendTransform(tf);
+
+        // Publish pitch roll based on IMU only
+        Eigen::Vector3d imu_ypr_eigen = trafo::QuatToEul(tf_data.rotation);
+        imu_ypr_eigen.x() = 0.0;  // the yaw value is not observable using IMU alone
+        geometry_msgs::Vector3Stamped imu_ypr;
+        imu_ypr.header.stamp = tf.header.stamp;
+        imu_ypr.header.frame_id = "FP_POI";
+        tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr.vector);
+        eul_imu_pub_.publish(imu_ypr);
+
+    }
+    // FP_POI -> FP_POISH
+    else if ((tf.child_frame_id == "FP_POISH") && (tf.header.frame_id == "FP_POI")) {
+        tf_br_.sendTransform(tf);
+        // Store TF if Nav2 mode is enabled
+        if (driver_params_.nav2_mode_) {
+            std::unique_lock<std::mutex> lock(tfs_.mutex_);
+            tfs_.poi_poish_ = std::make_unique<geometry_msgs::TransformStamped>(tf);
         }
-    } else if (nav_sat_fix.frame_id == "GNSS2") {
-        if (navsatfix_gnss2_pub_.getNumSubscribers() > 0) {
-            sensor_msgs::NavSatFix msg;
-            NavSatFixDataToMsg(nav_sat_fix, msg);
-            navsatfix_gnss2_pub_.publish(msg);
+    }
+    // FP_ECEF -> FP_ENU0
+    else if ((tf.child_frame_id == "FP_ENU0") && (tf.header.frame_id == "FP_ECEF")) {
+        static_br_.sendTransform(tf);
+        // Store TF if Nav2 mode is enabled
+        if (driver_params_.nav2_mode_) {
+            std::unique_lock<std::mutex> lock(tfs_.mutex_);
+            tfs_.ecef_enu0_ = std::make_unique<geometry_msgs::TransformStamped>(tf);
         }
     }
+    // Something else
+    else {
+        static_br_.sendTransform(tf);
+    }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+void FixpositionDriverNode::ProcessOdometryData(const OdometryData& odometry_data) {
+    switch (odometry_data.type) {
+        case OdometryData::Type::ODOMETRY:
+
+            if (odometry_data.valid) {
+                geometry_msgs::TransformStamped tf;
+                OdometryDataToTransformStamped(odometry_data, tf);
+                tf_br_.sendTransform(tf);
+            }
+
+            // Output jump warning
+            if (driver_params_.cov_warning_ && odometry_data.valid && jump_detector_.Check(odometry_data)) {
+                ROS_WARN(jump_detector_.warning_.c_str());
+                PublishJumpWarning(jump_detector_, jump_pub_);
+            }
+
+            break;
+
+        case OdometryData::Type::ODOMENU:
+            // Store FP_ENU0 -> FP_POI TF if Nav2 mode is selected
+            if (driver_params_.nav2_mode_ && odometry_data.valid) {
+                std::unique_lock<std::mutex> lock(tfs_.mutex_);
+                tfs_.enu0_poi_ = std::make_unique<geometry_msgs::TransformStamped>();
+                OdometryDataToTransformStamped(odometry_data, *tfs_.enu0_poi_);
+            }
+            break;
+
+        case OdometryData::Type::ODOMSH:
+            // Store FP_ECEF -> FP_POISH TF if Nav2 mode is selected
+            if (driver_params_.nav2_mode_ && odometry_data.valid) {
+                std::unique_lock<std::mutex> lock(tfs_.mutex_);
+                tfs_.ecef_poish_ = std::make_unique<geometry_msgs::TransformStamped>();
+                OdometryDataToTransformStamped(odometry_data, *tfs_.ecef_poish_);
+            }
+            break;
+
+        case OdometryData::Type::UNSPECIFIED:
+            break;
+    }
 }
 
+// ---------------------------------------------------------------------------------------------------------------------
+
+void FixpositionDriverNode::PublishNav2Tf() {
+    std::unique_lock<std::mutex> lock(tfs_.mutex_);
+    // We'll need all before we can start publishing the Nav2 TFs
+    if (!tfs_.ecef_enu0_ || !tfs_.poi_poish_ || !tfs_.ecef_poish_ || !tfs_.enu0_poi_) {
+        return;
+    }
+
+    // Publish FP_ECEF -> map
+    tfs_.ecef_enu0_->child_frame_id = "map";
+    static_br_.sendTransform(*tfs_.ecef_enu0_);
+
+    // Compute FP_ENU0 -> FP_POISH
+    // Extract translation and rotation from ECEFENU0
+    geometry_msgs::Vector3 trans_ecef_enu0 = tfs_.ecef_enu0_->transform.translation;
+    geometry_msgs::Quaternion rot_ecef_enu0 = tfs_.ecef_enu0_->transform.rotation;
+    Eigen::Vector3d t_ecef_enu0_;
+    t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z;
+    Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z);
+
+    // Extract translation and rotation from ECEFPOISH
+    geometry_msgs::Vector3 trans_ecef_poish = tfs_.ecef_poish_->transform.translation;
+    geometry_msgs::Quaternion rot_ecef_poish = tfs_.ecef_poish_->transform.rotation;
+    Eigen::Vector3d t_ecef_poish;
+    t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z;
+    Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z);
+
+    // Compute the ENU transformation
+    const Eigen::Vector3d t_enu0_poish = trafo::TfEnuEcef(t_ecef_poish, trafo::TfWgs84LlhEcef(t_ecef_enu0_));
+    const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish;
+
+    // Create tf2::Transform tf_ENU0POISH
+    tf2::Transform tf_ENU0POISH;
+    tf_ENU0POISH.setOrigin(tf2::Vector3(t_enu0_poish.x(), t_enu0_poish.y(), t_enu0_poish.z()));
+    tf2::Quaternion tf_q_enu0_poish(q_enu0_poish.x(), q_enu0_poish.y(), q_enu0_poish.z(), q_enu0_poish.w());
+    tf_ENU0POISH.setRotation(tf_q_enu0_poish);
+
+    // Publish map -> odom
+    // Multiply the transforms
+    tf2::Transform tf_ENU0POI;
+    tf2::fromMsg(tfs_.enu0_poi_->transform, tf_ENU0POI);
+    tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse();
+
+    // Create a new TransformStamped message
+    geometry_msgs::TransformStamped tfs_odom;
+    tfs_odom.header.stamp = ros::Time::now();
+    tfs_odom.header.frame_id = "map";
+    tfs_odom.child_frame_id = "odom";
+    tfs_odom.transform = tf2::toMsg(tf_combined);
+    tf_br_.sendTransform(tfs_odom);
+
+    // Publish odom -> base_link
+    geometry_msgs::TransformStamped tf_odom_base;
+    tf_odom_base.header.stamp = ros::Time::now();
+    tf_odom_base.header.frame_id = "odom";
+    tf_odom_base.child_frame_id = "base_link";
+    tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH);
+
+    // Send the transform
+    tf_br_.sendTransform(tf_odom_base);
+}
+
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
 
+using namespace fixposition;
+
 int main(int argc, char** argv) {
+#ifndef NDEBUG
+    fpsdk::common::app::StacktraceHelper stacktrace;
+    WARNING("***** Running debug build *****");
+#endif
+
+    bool ok = true;
+
+    // Initialise ROS, create node handle
     ros::init(argc, argv, "fixposition_driver");
-    ros::NodeHandle node_handle;
-    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
+    ros::NodeHandle node_handle("~");
 
-    fixposition::FixpositionDriverParams params;
+    // Redirect Fixposition SDK logging to ROS console
+    fpsdk::ros1::utils::RedirectLoggingToRosConsole();
 
-    if (fixposition::LoadParamsFromRos1("~", params)) {
-        ROS_INFO("Params Loaded!");
-        fixposition::FixpositionDriverNode node(params);
-        ROS_DEBUG("Starting node...");
+    // Say hello
+    HelloWorld();
 
-        node.Run();
-        ros::waitForShutdown();
+    // Load parameters
+    ROS_INFO("Loading parameters...");
+    DriverParams driver_params;
+    if (!LoadParamsFromRos1("~/driver", driver_params)) {
+        ROS_ERROR("Failed loading sensor params");
+        ok = false;
+    }
+    NodeParams node_params;
+    if (!LoadParamsFromRos1("~/node", node_params)) {
+        ROS_ERROR("Failed loading node params");
+        ok = false;
+    }
 
-        ROS_DEBUG("Exiting.");
-        return 0;
+    // Handle CTRL-C / SIGINT ourselves
+    fpsdk::common::app::SigIntHelper sigint;
+
+    // Start node
+    std::unique_ptr<FixpositionDriverNode> node;
+    if (ok) {
+        try {
+            node = std::make_unique<FixpositionDriverNode>(driver_params, node_params, node_handle);
+        } catch (const std::exception& ex) {
+            ROS_ERROR("Failed creating node: %s", ex.what());
+            ok = false;
+        }
+    }
+    if (ok) {
+        ROS_INFO("Starting node...");
+        if (node->StartNode()) {
+            ROS_INFO("main() spinning...");
+            // Use multiple spinner threads. Callback execute in one of them.
+            ros::AsyncSpinner spinner(4);
+            spinner.start();
+            sigint.WaitAbort();
+            spinner.stop();
+            ROS_INFO("main() stopping");
+        } else {
+            ROS_ERROR("Failed starting node");
+            ok = false;
+        }
+        node->StopNode();
+        node.reset();
+    }
+
+    // Are we happy?
+    if (ok) {
+        ROS_INFO("Done");
     } else {
-        ROS_ERROR("Params Loading Failed!");
-        ros::shutdown();
-        return 1;
+        ROS_FATAL("Ouch!");
     }
+
+    // Shutdown ROS
+    ros::shutdown();
+
+    exit(ok ? EXIT_SUCCESS : EXIT_FAILURE);
 }
+
+/* ****************************************************************************************************************** */
diff --git a/fixposition_driver_ros1/src/params.cpp b/fixposition_driver_ros1/src/params.cpp
index 0337b30..e90206d 100644
--- a/fixposition_driver_ros1/src/params.cpp
+++ b/fixposition_driver_ros1/src/params.cpp
@@ -1,128 +1,108 @@
 /**
- *  @file
- *  @brief Implementation of Parameter Loading
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Implementation of Parameter Loading
  */
 
+/* LIBC/STL */
+#include <cinttypes>
+
+/* EXTERNAL */
+#include <fpsdk_ros1/ext/ros.hpp>
+#include <fpsdk_ros1/ext/ros_console.hpp>
+#include <fpsdk_ros1/utils.hpp>
+
 /* PACKAGE */
-#include <fixposition_driver_ros1/params.hpp>
+#include "fixposition_driver_ros1/params.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-bool LoadParamsFromRos1(const std::string& ns, FpOutputParams& params) {
-    const std::string RATE = ns + "/rate";
-    const std::string RECONNECT_DELAY = ns + "/reconnect_delay";
-    const std::string TYPE = ns + "/type";
-    const std::string FORMATS = ns + "/formats";
-    const std::string IP = ns + "/ip";
-    const std::string PORT = ns + "/port";
-    const std::string BAUDRATE = ns + "/baudrate";
-    const std::string COV_WARNING = ns + "/cov_warning";
-    const std::string NAV2_MODE = ns + "/nav2_mode";
+using namespace fpsdk::ros1;
+
+bool LoadParamsFromRos1(const std::string& ns, DriverParams& params) {
+    bool ok = true;
+    ROS_INFO("DriverParams: loading from %s", ns.c_str());
 
-    // read parameters
-    if (!ros::param::get(RATE, params.rate)) {
-        params.rate = 100;
-        ROS_WARN("Using Default Rate : %d", params.rate);
+    if (!utils::LoadRosParam(ns + "/stream", params.stream_)) {
+        ROS_WARN("Failed loading %s/stream param", ns.c_str());
+        ok = false;
     }
-    if (!ros::param::get(RECONNECT_DELAY, params.reconnect_delay)) {
-        params.reconnect_delay = 5.0;
-        ROS_WARN("Using Default Reconnect Delay : %f", params.reconnect_delay);
+    if (!utils::LoadRosParam(ns + "/reconnect_delay", params.reconnect_delay_)) {
+        ROS_WARN("Failed loading %s/reconnect_delay param", ns.c_str());
+        ok = false;
     }
-    if (!ros::param::get(COV_WARNING, params.cov_warning)) {
-        params.cov_warning = false;
-        ROS_WARN("Using Default Covariance Warning option : %i", params.cov_warning);
+    if (!utils::LoadRosParam(ns + "/messages", params.messages_)) {
+        ROS_WARN("Failed loading %s/messages param", ns.c_str());
+        ok = false;
     }
-    if (!ros::param::get(NAV2_MODE, params.nav2_mode)) {
-        params.nav2_mode = false;
-        ROS_WARN("Using Default Nav2 mode option : %i", params.nav2_mode);
+    std::string epoch_str;
+    if (!utils::LoadRosParam(ns + "/nmea_epoch", epoch_str)) {
+        ROS_WARN("Failed loading %s/nmea_epoch param", ns.c_str());
+        ok = false;
     }
-
-    std::string type_str;
-    if (!ros::param::get(TYPE, type_str)) {
-        return false;
+    if (!StrToEpoch(epoch_str, params.nmea_epoch_)) {
+        ROS_WARN("Bad value for %s/nmea_epoch param", ns.c_str());
+        ok = false;
     }
-    if (type_str == "tcp") {
-        params.type = INPUT_TYPE::TCP;
-    } else if (type_str == "serial") {
-        params.type = INPUT_TYPE::SERIAL;
-    } else {
-        ROS_ERROR("Input type has to be tcp or serial!");
-        return false;
+    if (!utils::LoadRosParam(ns + "/raw_output", params.raw_output_)) {
+        ROS_WARN("Failed loading %s/raw_output param", ns.c_str());
+        ok = false;
     }
-
-    if (!ros::param::get(FORMATS, params.formats)) {
-        params.formats = {"ODOMETRY", "RAWIMU", "CORRIMU", "TF"};
+    if (!utils::LoadRosParam(ns + "/cov_warning", params.cov_warning_)) {
+        ROS_WARN("Failed loading %s/cov_warning param", ns.c_str());
+        ok = false;
     }
-
-    for (size_t i = 0; i < params.formats.size(); i++) {
-        ROS_INFO("%s[%ld] : %s", FORMATS.c_str(), i, params.formats.at(i).c_str());
+    if (!utils::LoadRosParam(ns + "/nav2_mode", params.nav2_mode_)) {
+        ROS_WARN("Failed loading %s/nav2_mode param", ns.c_str());
+        ok = false;
     }
 
-    if (params.type == INPUT_TYPE::TCP) {
-        if (!ros::param::get(IP, params.ip)) {
-            // default value for IP
-            params.ip = "10.0.2.1";
-            ROS_WARN("Using Default IP : %s", params.ip.c_str());
-        }
-        if (!ros::param::get(PORT, params.port)) {
-            // default value for TCP port
-            params.port = "21000";
-            ROS_WARN("Using Default Port : %s", params.port.c_str());
-        }
-        ROS_INFO("%s : %s", IP.c_str(), params.ip.c_str());
-        ROS_INFO("%s : %s", PORT.c_str(), params.port.c_str());
-    } else if (params.type == INPUT_TYPE::SERIAL) {
-        if (!ros::param::get(BAUDRATE, params.baudrate)) {
-            // default value for baudrate
-            params.baudrate = 115200;
-        }
-        if (!ros::param::get(PORT, params.port)) {
-            // default value for serial port
-            params.port = "/dev/ttyUSB0";
-        }
-        ROS_INFO("%s : %d", BAUDRATE.c_str(), params.baudrate);
-        ROS_INFO("%s : %s", PORT.c_str(), params.port.c_str());
+    ROS_INFO("DriverParams: stream=%s", params.stream_.c_str());
+    ROS_INFO("DriverParams: reconnect_delay=%.1f", params.reconnect_delay_);
+    for (std::size_t ix = 0; ix < params.messages_.size(); ix++) {
+        ROS_INFO("DriverParams: messages[%" PRIuMAX "]=%s", ix, params.messages_[ix].c_str());
     }
+    ROS_INFO("DriverParams: nmea_epoch=%s", epoch_str.c_str());
+    ROS_INFO("DriverParams: raw_output=%s", params.raw_output_ ? "true" : "false");
+    ROS_INFO("DriverParams: cov_warning=%s", params.cov_warning_ ? "true" : "false");
+    ROS_INFO("DriverParams: nav2_mode=%s", params.nav2_mode_ ? "true" : "false");
 
-    return true;
+    return ok;
 }
 
-bool LoadParamsFromRos1(const std::string& ns, CustomerInputParams& params) {
-    const std::string SPEED_TOPIC = ns + "/speed_topic";
-    if (!ros::param::get(SPEED_TOPIC, params.speed_topic)) {
-        // default value for the topic name
-        params.speed_topic = "/fixposition/speed";
-        ROS_WARN("Using Default Speed Topic : %s", params.speed_topic.c_str());
-    }
-    ROS_INFO("%s : %s", SPEED_TOPIC.c_str(), params.speed_topic.c_str());
-
-    const std::string RTCM_TOPIC = ns + "/rtcm_topic";
-    if (!ros::param::get(RTCM_TOPIC, params.rtcm_topic)) {
-        // default value for the topic name
-        params.rtcm_topic = "/fixposition/rtcm";
-        ROS_WARN("Using Default Rtcm Topic : %s", params.rtcm_topic.c_str());
-    }
-    ROS_INFO("%s : %s", RTCM_TOPIC.c_str(), params.rtcm_topic.c_str());
-
-    return true;
-}
+// ---------------------------------------------------------------------------------------------------------------------
 
-bool LoadParamsFromRos1(const std::string& ns, FixpositionDriverParams& params) {
+bool LoadParamsFromRos1(const std::string& ns, NodeParams& params) {
     bool ok = true;
+    ROS_INFO("NodeParams: loading from %s", ns.c_str());
+
+    if (!utils::LoadRosParam(ns + "/output_ns", params.output_ns_)) {
+        ROS_WARN("Failed loading %s/output_ns param", ns.c_str());
+        ok = false;
+    }
+    if (!utils::LoadRosParam(ns + "/speed_topic", params.speed_topic_)) {
+        ROS_WARN("Failed loading %s/speed_topic param", ns.c_str());
+        ok = false;
+    }
+    if (!utils::LoadRosParam(ns + "/corr_topic", params.corr_topic_)) {
+        ROS_WARN("Failed loading %s/corr_topic param", ns.c_str());
+        ok = false;
+    }
 
-    ok &= LoadParamsFromRos1(ns + "fp_output", params.fp_output);
-    ok &= LoadParamsFromRos1(ns + "customer_input", params.customer_input);
+    ROS_INFO("NodeParams: output_ns=%s", params.output_ns_.c_str());
+    ROS_INFO("NodeParams: speed_topic=%s", params.speed_topic_.c_str());
+    ROS_INFO("NodeParams: corr_topic=%s", params.corr_topic_.c_str());
 
     return ok;
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt
index 96205b0..71b1a98 100644
--- a/fixposition_driver_ros2/CMakeLists.txt
+++ b/fixposition_driver_ros2/CMakeLists.txt
@@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5)
 project(fixposition_driver_ros2)
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic\
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Werror\
     -Wshadow -Wunused-parameter -Wformat -Wpointer-arith")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3")
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
@@ -23,49 +23,20 @@ find_package(tf2_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(fixposition_driver_lib REQUIRED)
+find_package(fixposition_driver_msgs REQUIRED)
 find_package(rtcm_msgs REQUIRED)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  msg/Speed.msg
-  msg/Gnsssats.msg
-  msg/NMEA.msg
-  msg/WheelSensor.msg
-  msg/fpa/GNSSANT.msg
-  msg/fpa/GNSSCORR.msg
-  msg/fpa/IMUBIAS.msg
-  msg/fpa/EOE.msg
-  msg/fpa/LLH.msg
-  msg/fpa/ODOMENU.msg
-  msg/fpa/ODOMETRY.msg
-  msg/fpa/ODOMSH.msg
-  msg/fpa/ODOMSTATUS.msg
-  msg/fpa/TEXT.msg
-  msg/fpa/TP.msg
-  msg/nmea/GPGGA.msg
-  msg/nmea/GPGLL.msg
-  msg/nmea/GNGSA.msg
-  msg/nmea/GPGST.msg
-  msg/nmea/GXGSV.msg
-  msg/nmea/GPHDT.msg
-  msg/nmea/GPRMC.msg
-  msg/nmea/GPVTG.msg
-  msg/nmea/GPZDA.msg
-  msg/extras/COVWARN.msg
-  DEPENDENCIES
-  std_msgs
-  nav_msgs
-  geometry_msgs
-  builtin_interfaces
-)
-ament_export_dependencies(rosidl_default_runtime)
+find_package(fpsdk_common REQUIRED)
+find_package(fpsdk_ros2 REQUIRED)
 
 include_directories(
   include
   ${sensor_msgs_INCLUDE_DIR}
-  ${fixposition_driver_lib_INCLUDE_DIR}
+  ${fixposition_driver_lib_INCLUDE_DIRS}
+  ${fixposition_driver_msgs_INCLUDE_DIRS}
   ${rtcm_msgs_INCLUDE_DIR}
   ${EIGEN3_INCLUDE_DIR}
   ${Boost_INCLUDE_DIR}
+  ${fpsdk_common_INLCUDE_DIRS} ${fpsdk_ros2_INLCUDE_DIRS}
 )
 
 add_executable(
@@ -75,22 +46,13 @@ add_executable(
   src/data_to_ros2.cpp
 )
 
-if($ENV{ROS_DISTRO} MATCHES "humble|jazzy|rolling")
-  rosidl_get_typesupport_target(
-    cpp_typesupport_target
-    ${PROJECT_NAME} "rosidl_typesupport_cpp"
-  )
-else()
-  message(FATAL_ERROR "Unsupported ROS_DISTRO")
-endif()
-
 target_link_libraries(
   ${PROJECT_NAME}_exec
   ${fixposition_driver_lib_LIBRARIES}
   ${rtcm_msgs_LIBRARIES}
   ${Boost_LIBRARIES}
   ${EIGEN3_LIBRARIES}
-  ${cpp_typesupport_target}
+  ${fpsdk_common_LIBRARIES} ${fpsdk_ros2_LIBRARIES}
   pthread
 )
 
@@ -109,7 +71,9 @@ install(DIRECTORY
   "launch"
   DESTINATION share/${PROJECT_NAME}/
 )
-ament_target_dependencies(${PROJECT_NAME}_exec rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen fixposition_driver_lib rtcm_msgs tf2_geometry_msgs)
+ament_target_dependencies(${PROJECT_NAME}_exec
+    rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen
+    fixposition_driver_lib fixposition_driver_msgs rtcm_msgs tf2_geometry_msgs)
 
 # define ament package for this project
 ament_package()
diff --git a/fixposition_driver_ros2/Doxyfile b/fixposition_driver_ros2/Doxyfile
deleted file mode 100644
index 5f2c5e8..0000000
--- a/fixposition_driver_ros2/Doxyfile
+++ /dev/null
@@ -1,2427 +0,0 @@
-# Doxyfile 1.8.11
-
-# This file describes the settings to be used by the documentation system
-# doxygen (www.doxygen.org) for a project.
-#
-# All text after a double hash (##) is considered a comment and is placed in
-# front of the TAG it is preceding.
-#
-# All text after a single hash (#) is considered a comment and will be ignored.
-# The format is:
-# TAG = value [value, ...]
-# For lists, items can also be appended using:
-# TAG += value [value, ...]
-# Values that contain spaces should be placed between quotes (\" \").
-
-#---------------------------------------------------------------------------
-# Project related configuration options
-#---------------------------------------------------------------------------
-
-# This tag specifies the encoding used for all characters in the config file
-# that follow. The default is UTF-8 which is also the encoding used for all text
-# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
-# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
-# for the list of possible encodings.
-# The default value is: UTF-8.
-
-DOXYFILE_ENCODING      = UTF-8
-
-# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
-# double-quotes, unless you are using Doxywizard) that should identify the
-# project for which the documentation is generated. This name is used in the
-# title of most generated pages and in a few other places.
-# The default value is: My Project.
-
-PROJECT_NAME           = "Fixposition Driver ROS2"
-
-# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
-# could be handy for archiving the generated documentation or if some version
-# control system is used.
-
-PROJECT_NUMBER         =
-
-# Using the PROJECT_BRIEF tag one can provide an optional one line description
-# for a project that appears at the top of each page and should give viewer a
-# quick idea about the purpose of the project. Keep the description short.
-
-PROJECT_BRIEF          =
-
-# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
-# in the documentation. The maximum height of the logo should not exceed 55
-# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
-# the logo to the output directory.
-
-PROJECT_LOGO           =
-
-# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
-# into which the generated documentation will be written. If a relative path is
-# entered, it will be relative to the location where doxygen was started. If
-# left blank the current directory will be used.
-
-OUTPUT_DIRECTORY       = doc_gen
-
-# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
-# directories (in 2 levels) under the output directory of each output format and
-# will distribute the generated files over these directories. Enabling this
-# option can be useful when feeding doxygen a huge amount of source files, where
-# putting all generated files in the same directory would otherwise causes
-# performance problems for the file system.
-# The default value is: NO.
-
-CREATE_SUBDIRS         = NO
-
-# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
-# characters to appear in the names of generated files. If set to NO, non-ASCII
-# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
-# U+3044.
-# The default value is: NO.
-
-ALLOW_UNICODE_NAMES    = NO
-
-# The OUTPUT_LANGUAGE tag is used to specify the language in which all
-# documentation generated by doxygen is written. Doxygen will use this
-# information to generate all constant output in the proper language.
-# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
-# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
-# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
-# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
-# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
-# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
-# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
-# Ukrainian and Vietnamese.
-# The default value is: English.
-
-OUTPUT_LANGUAGE        = English
-
-# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
-# descriptions after the members that are listed in the file and class
-# documentation (similar to Javadoc). Set to NO to disable this.
-# The default value is: YES.
-
-BRIEF_MEMBER_DESC      = YES
-
-# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
-# description of a member or function before the detailed description
-#
-# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
-# brief descriptions will be completely suppressed.
-# The default value is: YES.
-
-REPEAT_BRIEF           = YES
-
-# This tag implements a quasi-intelligent brief description abbreviator that is
-# used to form the text in various listings. Each string in this list, if found
-# as the leading text of the brief description, will be stripped from the text
-# and the result, after processing the whole list, is used as the annotated
-# text. Otherwise, the brief description is used as-is. If left blank, the
-# following values are used ($name is automatically replaced with the name of
-# the entity):The $name class, The $name widget, The $name file, is, provides,
-# specifies, contains, represents, a, an and the.
-
-ABBREVIATE_BRIEF       =
-
-# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
-# doxygen will generate a detailed section even if there is only a brief
-# description.
-# The default value is: NO.
-
-ALWAYS_DETAILED_SEC    = NO
-
-# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
-# inherited members of a class in the documentation of that class as if those
-# members were ordinary class members. Constructors, destructors and assignment
-# operators of the base classes will not be shown.
-# The default value is: NO.
-
-INLINE_INHERITED_MEMB  = NO
-
-# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
-# before files name in the file list and in the header files. If set to NO the
-# shortest path that makes the file name unique will be used
-# The default value is: YES.
-
-FULL_PATH_NAMES        = YES
-
-# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
-# Stripping is only done if one of the specified strings matches the left-hand
-# part of the path. The tag can be used to show relative paths in the file list.
-# If left blank the directory from which doxygen is run is used as the path to
-# strip.
-#
-# Note that you can specify absolute paths here, but also relative paths, which
-# will be relative from the directory where doxygen is started.
-# This tag requires that the tag FULL_PATH_NAMES is set to YES.
-
-STRIP_FROM_PATH        =
-
-# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
-# path mentioned in the documentation of a class, which tells the reader which
-# header file to include in order to use a class. If left blank only the name of
-# the header file containing the class definition is used. Otherwise one should
-# specify the list of include paths that are normally passed to the compiler
-# using the -I flag.
-
-STRIP_FROM_INC_PATH    =
-
-# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
-# less readable) file names. This can be useful is your file systems doesn't
-# support long names like on DOS, Mac, or CD-ROM.
-# The default value is: NO.
-
-SHORT_NAMES            = NO
-
-# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
-# first line (until the first dot) of a Javadoc-style comment as the brief
-# description. If set to NO, the Javadoc-style will behave just like regular Qt-
-# style comments (thus requiring an explicit @brief command for a brief
-# description.)
-# The default value is: NO.
-
-JAVADOC_AUTOBRIEF      = NO
-
-# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
-# line (until the first dot) of a Qt-style comment as the brief description. If
-# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
-# requiring an explicit \brief command for a brief description.)
-# The default value is: NO.
-
-QT_AUTOBRIEF           = NO
-
-# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
-# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
-# a brief description. This used to be the default behavior. The new default is
-# to treat a multi-line C++ comment block as a detailed description. Set this
-# tag to YES if you prefer the old behavior instead.
-#
-# Note that setting this tag to YES also means that rational rose comments are
-# not recognized any more.
-# The default value is: NO.
-
-MULTILINE_CPP_IS_BRIEF = NO
-
-# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
-# documentation from any documented member that it re-implements.
-# The default value is: YES.
-
-INHERIT_DOCS           = YES
-
-# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
-# page for each member. If set to NO, the documentation of a member will be part
-# of the file/class/namespace that contains it.
-# The default value is: NO.
-
-SEPARATE_MEMBER_PAGES  = NO
-
-# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
-# uses this value to replace tabs by spaces in code fragments.
-# Minimum value: 1, maximum value: 16, default value: 4.
-
-TAB_SIZE               = 4
-
-# This tag can be used to specify a number of aliases that act as commands in
-# the documentation. An alias has the form:
-# name=value
-# For example adding
-# "sideeffect=@par Side Effects:\n"
-# will allow you to put the command \sideeffect (or @sideeffect) in the
-# documentation, which will result in a user-defined paragraph with heading
-# "Side Effects:". You can put \n's in the value part of an alias to insert
-# newlines.
-
-ALIASES                =
-
-# This tag can be used to specify a number of word-keyword mappings (TCL only).
-# A mapping has the form "name=value". For example adding "class=itcl::class"
-# will allow you to use the command class in the itcl::class meaning.
-
-TCL_SUBST              =
-
-# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
-# only. Doxygen will then generate output that is more tailored for C. For
-# instance, some of the names that are used will be different. The list of all
-# members will be omitted, etc.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_FOR_C  = NO
-
-# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
-# Python sources only. Doxygen will then generate output that is more tailored
-# for that language. For instance, namespaces will be presented as packages,
-# qualified scopes will look different, etc.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_JAVA   = NO
-
-# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
-# sources. Doxygen will then generate output that is tailored for Fortran.
-# The default value is: NO.
-
-OPTIMIZE_FOR_FORTRAN   = NO
-
-# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
-# sources. Doxygen will then generate output that is tailored for VHDL.
-# The default value is: NO.
-
-OPTIMIZE_OUTPUT_VHDL   = NO
-
-# Doxygen selects the parser to use depending on the extension of the files it
-# parses. With this tag you can assign which parser to use for a given
-# extension. Doxygen has a built-in mapping, but you can override or extend it
-# using this tag. The format is ext=language, where ext is a file extension, and
-# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
-# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
-# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
-# Fortran. In the later case the parser tries to guess whether the code is fixed
-# or free formatted code, this is the default for Fortran type files), VHDL. For
-# instance to make doxygen treat .inc files as Fortran files (default is PHP),
-# and .f files as C (default is Fortran), use: inc=Fortran f=C.
-#
-# Note: For files without extension you can use no_extension as a placeholder.
-#
-# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
-# the files are not read by doxygen.
-
-EXTENSION_MAPPING      =
-
-# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
-# according to the Markdown format, which allows for more readable
-# documentation. See http://daringfireball.net/projects/markdown/ for details.
-# The output of markdown processing is further processed by doxygen, so you can
-# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
-# case of backward compatibilities issues.
-# The default value is: YES.
-
-MARKDOWN_SUPPORT       = YES
-
-# When enabled doxygen tries to link words that correspond to documented
-# classes, or namespaces to their corresponding documentation. Such a link can
-# be prevented in individual cases by putting a % sign in front of the word or
-# globally by setting AUTOLINK_SUPPORT to NO.
-# The default value is: YES.
-
-AUTOLINK_SUPPORT       = YES
-
-# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
-# to include (a tag file for) the STL sources as input, then you should set this
-# tag to YES in order to let doxygen match functions declarations and
-# definitions whose arguments contain STL classes (e.g. func(std::string);
-# versus func(std::string) {}). This also make the inheritance and collaboration
-# diagrams that involve STL classes more complete and accurate.
-# The default value is: NO.
-
-BUILTIN_STL_SUPPORT    = NO
-
-# If you use Microsoft's C++/CLI language, you should set this option to YES to
-# enable parsing support.
-# The default value is: NO.
-
-CPP_CLI_SUPPORT        = NO
-
-# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
-# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
-# will parse them like normal C++ but will assume all classes use public instead
-# of private inheritance when no explicit protection keyword is present.
-# The default value is: NO.
-
-SIP_SUPPORT            = NO
-
-# For Microsoft's IDL there are propget and propput attributes to indicate
-# getter and setter methods for a property. Setting this option to YES will make
-# doxygen to replace the get and set methods by a property in the documentation.
-# This will only work if the methods are indeed getting or setting a simple
-# type. If this is not the case, or you want to show the methods anyway, you
-# should set this option to NO.
-# The default value is: YES.
-
-IDL_PROPERTY_SUPPORT   = YES
-
-# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
-# tag is set to YES then doxygen will reuse the documentation of the first
-# member in the group (if any) for the other members of the group. By default
-# all members of a group must be documented explicitly.
-# The default value is: NO.
-
-DISTRIBUTE_GROUP_DOC   = NO
-
-# If one adds a struct or class to a group and this option is enabled, then also
-# any nested class or struct is added to the same group. By default this option
-# is disabled and one has to add nested compounds explicitly via \ingroup.
-# The default value is: NO.
-
-GROUP_NESTED_COMPOUNDS = NO
-
-# Set the SUBGROUPING tag to YES to allow class member groups of the same type
-# (for instance a group of public functions) to be put as a subgroup of that
-# type (e.g. under the Public Functions section). Set it to NO to prevent
-# subgrouping. Alternatively, this can be done per class using the
-# \nosubgrouping command.
-# The default value is: YES.
-
-SUBGROUPING            = YES
-
-# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
-# are shown inside the group in which they are included (e.g. using \ingroup)
-# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
-# and RTF).
-#
-# Note that this feature does not work in combination with
-# SEPARATE_MEMBER_PAGES.
-# The default value is: NO.
-
-INLINE_GROUPED_CLASSES = NO
-
-# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
-# with only public data fields or simple typedef fields will be shown inline in
-# the documentation of the scope in which they are defined (i.e. file,
-# namespace, or group documentation), provided this scope is documented. If set
-# to NO, structs, classes, and unions are shown on a separate page (for HTML and
-# Man pages) or section (for LaTeX and RTF).
-# The default value is: NO.
-
-INLINE_SIMPLE_STRUCTS  = NO
-
-# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
-# enum is documented as struct, union, or enum with the name of the typedef. So
-# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
-# with name TypeT. When disabled the typedef will appear as a member of a file,
-# namespace, or class. And the struct will be named TypeS. This can typically be
-# useful for C code in case the coding convention dictates that all compound
-# types are typedef'ed and only the typedef is referenced, never the tag name.
-# The default value is: NO.
-
-TYPEDEF_HIDES_STRUCT   = NO
-
-# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
-# cache is used to resolve symbols given their name and scope. Since this can be
-# an expensive process and often the same symbol appears multiple times in the
-# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
-# doxygen will become slower. If the cache is too large, memory is wasted. The
-# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
-# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
-# symbols. At the end of a run doxygen will report the cache usage and suggest
-# the optimal cache size from a speed point of view.
-# Minimum value: 0, maximum value: 9, default value: 0.
-
-LOOKUP_CACHE_SIZE      = 0
-
-#---------------------------------------------------------------------------
-# Build related configuration options
-#---------------------------------------------------------------------------
-
-# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
-# documentation are documented, even if no documentation was available. Private
-# class members and static file members will be hidden unless the
-# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
-# Note: This will also disable the warnings about undocumented members that are
-# normally produced when WARNINGS is set to YES.
-# The default value is: NO.
-
-EXTRACT_ALL            =YES
-
-# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
-# be included in the documentation.
-# The default value is: NO.
-
-EXTRACT_PRIVATE        = YES
-
-# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
-# scope will be included in the documentation.
-# The default value is: NO.
-
-EXTRACT_PACKAGE        = NO
-
-# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
-# included in the documentation.
-# The default value is: NO.
-
-EXTRACT_STATIC         = YES
-
-# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
-# locally in source files will be included in the documentation. If set to NO,
-# only classes defined in header files are included. Does not have any effect
-# for Java sources.
-# The default value is: YES.
-
-EXTRACT_LOCAL_CLASSES  = YES
-
-# This flag is only useful for Objective-C code. If set to YES, local methods,
-# which are defined in the implementation section but not in the interface are
-# included in the documentation. If set to NO, only methods in the interface are
-# included.
-# The default value is: NO.
-
-EXTRACT_LOCAL_METHODS  = NO
-
-# If this flag is set to YES, the members of anonymous namespaces will be
-# extracted and appear in the documentation as a namespace called
-# 'anonymous_namespace{file}', where file will be replaced with the base name of
-# the file that contains the anonymous namespace. By default anonymous namespace
-# are hidden.
-# The default value is: NO.
-
-EXTRACT_ANON_NSPACES   = NO
-
-# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
-# undocumented members inside documented classes or files. If set to NO these
-# members will be included in the various overviews, but no documentation
-# section is generated. This option has no effect if EXTRACT_ALL is enabled.
-# The default value is: NO.
-
-HIDE_UNDOC_MEMBERS     = NO
-
-# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
-# undocumented classes that are normally visible in the class hierarchy. If set
-# to NO, these classes will be included in the various overviews. This option
-# has no effect if EXTRACT_ALL is enabled.
-# The default value is: NO.
-
-HIDE_UNDOC_CLASSES     = NO
-
-# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
-# (class|struct|union) declarations. If set to NO, these declarations will be
-# included in the documentation.
-# The default value is: NO.
-
-HIDE_FRIEND_COMPOUNDS  = NO
-
-# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
-# documentation blocks found inside the body of a function. If set to NO, these
-# blocks will be appended to the function's detailed documentation block.
-# The default value is: NO.
-
-HIDE_IN_BODY_DOCS      = NO
-
-# The INTERNAL_DOCS tag determines if documentation that is typed after a
-# \internal command is included. If the tag is set to NO then the documentation
-# will be excluded. Set it to YES to include the internal documentation.
-# The default value is: NO.
-
-INTERNAL_DOCS          = NO
-
-# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
-# names in lower-case letters. If set to YES, upper-case letters are also
-# allowed. This is useful if you have classes or files whose names only differ
-# in case and if your file system supports case sensitive file names. Windows
-# and Mac users are advised to set this option to NO.
-# The default value is: system dependent.
-
-CASE_SENSE_NAMES       = YES
-
-# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
-# their full class and namespace scopes in the documentation. If set to YES, the
-# scope will be hidden.
-# The default value is: NO.
-
-HIDE_SCOPE_NAMES       = NO
-
-# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
-# append additional text to a page's title, such as Class Reference. If set to
-# YES the compound reference will be hidden.
-# The default value is: NO.
-
-HIDE_COMPOUND_REFERENCE= NO
-
-# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
-# the files that are included by a file in the documentation of that file.
-# The default value is: YES.
-
-SHOW_INCLUDE_FILES     = YES
-
-# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
-# grouped member an include statement to the documentation, telling the reader
-# which file to include in order to use the member.
-# The default value is: NO.
-
-SHOW_GROUPED_MEMB_INC  = NO
-
-# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
-# files with double quotes in the documentation rather than with sharp brackets.
-# The default value is: NO.
-
-FORCE_LOCAL_INCLUDES   = NO
-
-# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
-# documentation for inline members.
-# The default value is: YES.
-
-INLINE_INFO            = YES
-
-# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
-# (detailed) documentation of file and class members alphabetically by member
-# name. If set to NO, the members will appear in declaration order.
-# The default value is: YES.
-
-SORT_MEMBER_DOCS       = YES
-
-# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
-# descriptions of file, namespace and class members alphabetically by member
-# name. If set to NO, the members will appear in declaration order. Note that
-# this will also influence the order of the classes in the class list.
-# The default value is: NO.
-
-SORT_BRIEF_DOCS        = NO
-
-# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
-# (brief and detailed) documentation of class members so that constructors and
-# destructors are listed first. If set to NO the constructors will appear in the
-# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
-# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
-# member documentation.
-# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
-# detailed member documentation.
-# The default value is: NO.
-
-SORT_MEMBERS_CTORS_1ST = NO
-
-# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
-# of group names into alphabetical order. If set to NO the group names will
-# appear in their defined order.
-# The default value is: NO.
-
-SORT_GROUP_NAMES       = NO
-
-# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
-# fully-qualified names, including namespaces. If set to NO, the class list will
-# be sorted only by class name, not including the namespace part.
-# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
-# Note: This option applies only to the class list, not to the alphabetical
-# list.
-# The default value is: NO.
-
-SORT_BY_SCOPE_NAME     = NO
-
-# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
-# type resolution of all parameters of a function it will reject a match between
-# the prototype and the implementation of a member function even if there is
-# only one candidate or it is obvious which candidate to choose by doing a
-# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
-# accept a match between prototype and implementation in such cases.
-# The default value is: NO.
-
-STRICT_PROTO_MATCHING  = NO
-
-# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
-# list. This list is created by putting \todo commands in the documentation.
-# The default value is: YES.
-
-GENERATE_TODOLIST      = YES
-
-# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
-# list. This list is created by putting \test commands in the documentation.
-# The default value is: YES.
-
-GENERATE_TESTLIST      = YES
-
-# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
-# list. This list is created by putting \bug commands in the documentation.
-# The default value is: YES.
-
-GENERATE_BUGLIST       = YES
-
-# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
-# the deprecated list. This list is created by putting \deprecated commands in
-# the documentation.
-# The default value is: YES.
-
-GENERATE_DEPRECATEDLIST= YES
-
-# The ENABLED_SECTIONS tag can be used to enable conditional documentation
-# sections, marked by \if <section_label> ... \endif and \cond <section_label>
-# ... \endcond blocks.
-
-ENABLED_SECTIONS       =
-
-# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
-# initial value of a variable or macro / define can have for it to appear in the
-# documentation. If the initializer consists of more lines than specified here
-# it will be hidden. Use a value of 0 to hide initializers completely. The
-# appearance of the value of individual variables and macros / defines can be
-# controlled using \showinitializer or \hideinitializer command in the
-# documentation regardless of this setting.
-# Minimum value: 0, maximum value: 10000, default value: 30.
-
-MAX_INITIALIZER_LINES  = 30
-
-# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
-# the bottom of the documentation of classes and structs. If set to YES, the
-# list will mention the files that were used to generate the documentation.
-# The default value is: YES.
-
-SHOW_USED_FILES        = YES
-
-# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
-# will remove the Files entry from the Quick Index and from the Folder Tree View
-# (if specified).
-# The default value is: YES.
-
-SHOW_FILES             = YES
-
-# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
-# page. This will remove the Namespaces entry from the Quick Index and from the
-# Folder Tree View (if specified).
-# The default value is: YES.
-
-SHOW_NAMESPACES        = YES
-
-# The FILE_VERSION_FILTER tag can be used to specify a program or script that
-# doxygen should invoke to get the current version for each file (typically from
-# the version control system). Doxygen will invoke the program by executing (via
-# popen()) the command command input-file, where command is the value of the
-# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
-# by doxygen. Whatever the program writes to standard output is used as the file
-# version. For an example see the documentation.
-
-FILE_VERSION_FILTER    =
-
-# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
-# by doxygen. The layout file controls the global structure of the generated
-# output files in an output format independent way. To create the layout file
-# that represents doxygen's defaults, run doxygen with the -l option. You can
-# optionally specify a file name after the option, if omitted DoxygenLayout.xml
-# will be used as the name of the layout file.
-#
-# Note that if you run doxygen from a directory containing a file called
-# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
-# tag is left empty.
-
-LAYOUT_FILE            =
-
-# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
-# the reference definitions. This must be a list of .bib files. The .bib
-# extension is automatically appended if omitted. This requires the bibtex tool
-# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
-# For LaTeX the style of the bibliography can be controlled using
-# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
-# search path. See also \cite for info how to create references.
-
-CITE_BIB_FILES         =
-
-#---------------------------------------------------------------------------
-# Configuration options related to warning and progress messages
-#---------------------------------------------------------------------------
-
-# The QUIET tag can be used to turn on/off the messages that are generated to
-# standard output by doxygen. If QUIET is set to YES this implies that the
-# messages are off.
-# The default value is: NO.
-
-QUIET                  = NO
-
-# The WARNINGS tag can be used to turn on/off the warning messages that are
-# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
-# this implies that the warnings are on.
-#
-# Tip: Turn warnings on while writing the documentation.
-# The default value is: YES.
-
-WARNINGS               = YES
-
-# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
-# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
-# will automatically be disabled.
-# The default value is: YES.
-
-WARN_IF_UNDOCUMENTED   = YES
-
-# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
-# potential errors in the documentation, such as not documenting some parameters
-# in a documented function, or documenting parameters that don't exist or using
-# markup commands wrongly.
-# The default value is: YES.
-
-WARN_IF_DOC_ERROR      = YES
-
-# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
-# are documented, but have no documentation for their parameters or return
-# value. If set to NO, doxygen will only warn about wrong or incomplete
-# parameter documentation, but not about the absence of documentation.
-# The default value is: NO.
-
-WARN_NO_PARAMDOC       = NO
-
-# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
-# a warning is encountered.
-# The default value is: NO.
-
-WARN_AS_ERROR          = NO
-
-# The WARN_FORMAT tag determines the format of the warning messages that doxygen
-# can produce. The string should contain the $file, $line, and $text tags, which
-# will be replaced by the file and line number from which the warning originated
-# and the warning text. Optionally the format may contain $version, which will
-# be replaced by the version of the file (if it could be obtained via
-# FILE_VERSION_FILTER)
-# The default value is: $file:$line: $text.
-
-WARN_FORMAT            = "$file:$line: $text"
-
-# The WARN_LOGFILE tag can be used to specify a file to which warning and error
-# messages should be written. If left blank the output is written to standard
-# error (stderr).
-
-WARN_LOGFILE           =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the input files
-#---------------------------------------------------------------------------
-
-# The INPUT tag is used to specify the files and/or directories that contain
-# documented source files. You may enter file names like myfile.cpp or
-# directories like /usr/src/myproject. Separate the files or directories with
-# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
-# Note: If this tag is empty the current directory is searched.
-
-INPUT                  = include src submodules README.md
-
-# This tag can be used to specify the character encoding of the source files
-# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
-# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
-# documentation (see: http://www.gnu.org/software/libiconv) for the list of
-# possible encodings.
-# The default value is: UTF-8.
-
-INPUT_ENCODING         = UTF-8
-
-# If the value of the INPUT tag contains directories, you can use the
-# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
-# *.h) to filter out the source-files in the directories.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# read by doxygen.
-#
-# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
-# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
-# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
-# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f, *.for, *.tcl,
-# *.vhd, *.vhdl, *.ucf, *.qsf, *.as and *.js.
-
-FILE_PATTERNS          =
-
-# The RECURSIVE tag can be used to specify whether or not subdirectories should
-# be searched for input files as well.
-# The default value is: NO.
-
-RECURSIVE              = YES
-
-# The EXCLUDE tag can be used to specify files and/or directories that should be
-# excluded from the INPUT source files. This way you can easily exclude a
-# subdirectory from a directory tree whose root is specified with the INPUT tag.
-#
-# Note that relative paths are relative to the directory from which doxygen is
-# run.
-
-EXCLUDE                =
-
-# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
-# directories that are symbolic links (a Unix file system feature) are excluded
-# from the input.
-# The default value is: NO.
-
-EXCLUDE_SYMLINKS       = NO
-
-# If the value of the INPUT tag contains directories, you can use the
-# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
-# certain files from those directories.
-#
-# Note that the wildcards are matched against the file with absolute path, so to
-# exclude all test directories for example use the pattern */test/*
-
-EXCLUDE_PATTERNS       =
-
-# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
-# (namespaces, classes, functions, etc.) that should be excluded from the
-# output. The symbol name can be a fully qualified name, a word, or if the
-# wildcard * is used, a substring. Examples: ANamespace, AClass,
-# AClass::ANamespace, ANamespace::*Test
-#
-# Note that the wildcards are matched against the file with absolute path, so to
-# exclude all test directories use the pattern */test/*
-
-EXCLUDE_SYMBOLS        =
-
-# The EXAMPLE_PATH tag can be used to specify one or more files or directories
-# that contain example code fragments that are included (see the \include
-# command).
-
-EXAMPLE_PATH           =
-
-# If the value of the EXAMPLE_PATH tag contains directories, you can use the
-# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
-# *.h) to filter out the source-files in the directories. If left blank all
-# files are included.
-
-EXAMPLE_PATTERNS       =
-
-# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
-# searched for input files to be used with the \include or \dontinclude commands
-# irrespective of the value of the RECURSIVE tag.
-# The default value is: NO.
-
-EXAMPLE_RECURSIVE      = NO
-
-# The IMAGE_PATH tag can be used to specify one or more files or directories
-# that contain images that are to be included in the documentation (see the
-# \image command).
-
-IMAGE_PATH             =
-
-# The INPUT_FILTER tag can be used to specify a program that doxygen should
-# invoke to filter for each input file. Doxygen will invoke the filter program
-# by executing (via popen()) the command:
-#
-# <filter> <input-file>
-#
-# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
-# name of an input file. Doxygen will then use the output that the filter
-# program writes to standard output. If FILTER_PATTERNS is specified, this tag
-# will be ignored.
-#
-# Note that the filter must not add or remove lines; it is applied before the
-# code is scanned, but not when the output code is generated. If lines are added
-# or removed, the anchors will not be placed correctly.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# properly processed by doxygen.
-
-INPUT_FILTER           =
-
-# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
-# basis. Doxygen will compare the file name with each pattern and apply the
-# filter if there is a match. The filters are a list of the form: pattern=filter
-# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
-# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
-# patterns match the file name, INPUT_FILTER is applied.
-#
-# Note that for custom extensions or not directly supported extensions you also
-# need to set EXTENSION_MAPPING for the extension otherwise the files are not
-# properly processed by doxygen.
-
-FILTER_PATTERNS        =
-
-# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
-# INPUT_FILTER) will also be used to filter the input files that are used for
-# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
-# The default value is: NO.
-
-FILTER_SOURCE_FILES    = NO
-
-# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
-# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
-# it is also possible to disable source filtering for a specific pattern using
-# *.ext= (so without naming a filter).
-# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
-
-FILTER_SOURCE_PATTERNS =
-
-# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
-# is part of the input, its contents will be placed on the main page
-# (index.html). This can be useful if you have a project on for instance GitHub
-# and want to reuse the introduction page also for the doxygen output.
-
-USE_MDFILE_AS_MAINPAGE = README.md
-
-#---------------------------------------------------------------------------
-# Configuration options related to source browsing
-#---------------------------------------------------------------------------
-
-# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
-# generated. Documented entities will be cross-referenced with these sources.
-#
-# Note: To get rid of all source code in the generated output, make sure that
-# also VERBATIM_HEADERS is set to NO.
-# The default value is: NO.
-
-SOURCE_BROWSER         = NO
-
-# Setting the INLINE_SOURCES tag to YES will include the body of functions,
-# classes and enums directly into the documentation.
-# The default value is: NO.
-
-INLINE_SOURCES         = NO
-
-# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
-# special comment blocks from generated source code fragments. Normal C, C++ and
-# Fortran comments will always remain visible.
-# The default value is: YES.
-
-STRIP_CODE_COMMENTS    = YES
-
-# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
-# function all documented functions referencing it will be listed.
-# The default value is: NO.
-
-REFERENCED_BY_RELATION = NO
-
-# If the REFERENCES_RELATION tag is set to YES then for each documented function
-# all documented entities called/used by that function will be listed.
-# The default value is: NO.
-
-REFERENCES_RELATION    = NO
-
-# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
-# to YES then the hyperlinks from functions in REFERENCES_RELATION and
-# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
-# link to the documentation.
-# The default value is: YES.
-
-REFERENCES_LINK_SOURCE = YES
-
-# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
-# source code will show a tooltip with additional information such as prototype,
-# brief description and links to the definition and documentation. Since this
-# will make the HTML file larger and loading of large files a bit slower, you
-# can opt to disable this feature.
-# The default value is: YES.
-# This tag requires that the tag SOURCE_BROWSER is set to YES.
-
-SOURCE_TOOLTIPS        = YES
-
-# If the USE_HTAGS tag is set to YES then the references to source code will
-# point to the HTML generated by the htags(1) tool instead of doxygen built-in
-# source browser. The htags tool is part of GNU's global source tagging system
-# (see http://www.gnu.org/software/global/global.html). You will need version
-# 4.8.6 or higher.
-#
-# To use it do the following:
-# - Install the latest version of global
-# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
-# - Make sure the INPUT points to the root of the source tree
-# - Run doxygen as normal
-#
-# Doxygen will invoke htags (and that will in turn invoke gtags), so these
-# tools must be available from the command line (i.e. in the search path).
-#
-# The result: instead of the source browser generated by doxygen, the links to
-# source code will now point to the output of htags.
-# The default value is: NO.
-# This tag requires that the tag SOURCE_BROWSER is set to YES.
-
-USE_HTAGS              = NO
-
-# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
-# verbatim copy of the header file for each class for which an include is
-# specified. Set to NO to disable this.
-# See also: Section \class.
-# The default value is: YES.
-
-VERBATIM_HEADERS       = YES
-
-# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
-# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
-# cost of reduced performance. This can be particularly helpful with template
-# rich C++ code for which doxygen's built-in parser lacks the necessary type
-# information.
-# Note: The availability of this option depends on whether or not doxygen was
-# generated with the -Duse-libclang=ON option for CMake.
-# The default value is: NO.
-
-CLANG_ASSISTED_PARSING = NO
-
-# If clang assisted parsing is enabled you can provide the compiler with command
-# line options that you would normally use when invoking the compiler. Note that
-# the include paths will already be set by doxygen for the files and directories
-# specified with INPUT and INCLUDE_PATH.
-# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
-
-CLANG_OPTIONS          =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the alphabetical class index
-#---------------------------------------------------------------------------
-
-# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
-# compounds will be generated. Enable this if the project contains a lot of
-# classes, structs, unions or interfaces.
-# The default value is: YES.
-
-ALPHABETICAL_INDEX     = YES
-
-# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
-# which the alphabetical index list will be split.
-# Minimum value: 1, maximum value: 20, default value: 5.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-COLS_IN_ALPHA_INDEX    = 5
-
-# In case all classes in a project start with a common prefix, all classes will
-# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
-# can be used to specify a prefix (or a list of prefixes) that should be ignored
-# while generating the index headers.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-IGNORE_PREFIX          =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the HTML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
-# The default value is: YES.
-
-GENERATE_HTML          = YES
-
-# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: html.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_OUTPUT            = html
-
-# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
-# generated HTML page (for example: .htm, .php, .asp).
-# The default value is: .html.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_FILE_EXTENSION    = .html
-
-# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
-# each generated HTML page. If the tag is left blank doxygen will generate a
-# standard header.
-#
-# To get valid HTML the header file that includes any scripts and style sheets
-# that doxygen needs, which is dependent on the configuration options used (e.g.
-# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
-# default header using
-# doxygen -w html new_header.html new_footer.html new_stylesheet.css
-# YourConfigFile
-# and then modify the file new_header.html. See also section "Doxygen usage"
-# for information on how to generate the default header that doxygen normally
-# uses.
-# Note: The header is subject to change so you typically have to regenerate the
-# default header when upgrading to a newer version of doxygen. For a description
-# of the possible markers and block names see the documentation.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_HEADER            =
-
-# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
-# generated HTML page. If the tag is left blank doxygen will generate a standard
-# footer. See HTML_HEADER for more information on how to generate a default
-# footer and what special commands can be used inside the footer. See also
-# section "Doxygen usage" for information on how to generate the default footer
-# that doxygen normally uses.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_FOOTER            =
-
-# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
-# sheet that is used by each HTML page. It can be used to fine-tune the look of
-# the HTML output. If left blank doxygen will generate a default style sheet.
-# See also section "Doxygen usage" for information on how to generate the style
-# sheet that doxygen normally uses.
-# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
-# it is more robust and this tag (HTML_STYLESHEET) will in the future become
-# obsolete.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_STYLESHEET        =
-
-# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
-# cascading style sheets that are included after the standard style sheets
-# created by doxygen. Using this option one can overrule certain style aspects.
-# This is preferred over using HTML_STYLESHEET since it does not replace the
-# standard style sheet and is therefore more robust against future updates.
-# Doxygen will copy the style sheet files to the output directory.
-# Note: The order of the extra style sheet files is of importance (e.g. the last
-# style sheet in the list overrules the setting of the previous ones in the
-# list). For an example see the documentation.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_EXTRA_STYLESHEET  =
-
-# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
-# other source files which should be copied to the HTML output directory. Note
-# that these files will be copied to the base HTML output directory. Use the
-# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
-# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
-# files will be copied as-is; there are no commands or markers available.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_EXTRA_FILES       =
-
-# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
-# will adjust the colors in the style sheet and background images according to
-# this color. Hue is specified as an angle on a colorwheel, see
-# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
-# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
-# purple, and 360 is red again.
-# Minimum value: 0, maximum value: 359, default value: 220.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_HUE    = 220
-
-# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
-# in the HTML output. For a value of 0 the output will use grayscales only. A
-# value of 255 will produce the most vivid colors.
-# Minimum value: 0, maximum value: 255, default value: 100.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_SAT    = 100
-
-# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
-# luminance component of the colors in the HTML output. Values below 100
-# gradually make the output lighter, whereas values above 100 make the output
-# darker. The value divided by 100 is the actual gamma applied, so 80 represents
-# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
-# change the gamma.
-# Minimum value: 40, maximum value: 240, default value: 80.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_COLORSTYLE_GAMMA  = 80
-
-# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
-# page will contain the date and time when the page was generated. Setting this
-# to YES can help to show when doxygen was last run and thus if the
-# documentation is up to date.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_TIMESTAMP         = NO
-
-# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
-# documentation will contain sections that can be hidden and shown after the
-# page has loaded.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_DYNAMIC_SECTIONS  = NO
-
-# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
-# shown in the various tree structured indices initially; the user can expand
-# and collapse entries dynamically later on. Doxygen will expand the tree to
-# such a level that at most the specified number of entries are visible (unless
-# a fully collapsed tree already exceeds this amount). So setting the number of
-# entries 1 will produce a full collapsed tree by default. 0 is a special value
-# representing an infinite number of entries and will result in a full expanded
-# tree by default.
-# Minimum value: 0, maximum value: 9999, default value: 100.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-HTML_INDEX_NUM_ENTRIES = 100
-
-# If the GENERATE_DOCSET tag is set to YES, additional index files will be
-# generated that can be used as input for Apple's Xcode 3 integrated development
-# environment (see: http://developer.apple.com/tools/xcode/), introduced with
-# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
-# Makefile in the HTML output directory. Running make will produce the docset in
-# that directory and running make install will install the docset in
-# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
-# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
-# for more information.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_DOCSET        = NO
-
-# This tag determines the name of the docset feed. A documentation feed provides
-# an umbrella under which multiple documentation sets from a single provider
-# (such as a company or product suite) can be grouped.
-# The default value is: Doxygen generated docs.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_FEEDNAME        = "Doxygen generated docs"
-
-# This tag specifies a string that should uniquely identify the documentation
-# set bundle. This should be a reverse domain-name style string, e.g.
-# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_BUNDLE_ID       = org.doxygen.Project
-
-# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
-# the documentation publisher. This should be a reverse domain-name style
-# string, e.g. com.mycompany.MyDocSet.documentation.
-# The default value is: org.doxygen.Publisher.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
-
-# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
-# The default value is: Publisher.
-# This tag requires that the tag GENERATE_DOCSET is set to YES.
-
-DOCSET_PUBLISHER_NAME  = Publisher
-
-# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
-# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
-# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
-# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
-# Windows.
-#
-# The HTML Help Workshop contains a compiler that can convert all HTML output
-# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
-# files are now used as the Windows 98 help format, and will replace the old
-# Windows help format (.hlp) on all Windows platforms in the future. Compressed
-# HTML files also contain an index, a table of contents, and you can search for
-# words in the documentation. The HTML workshop also contains a viewer for
-# compressed HTML files.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_HTMLHELP      = NO
-
-# The CHM_FILE tag can be used to specify the file name of the resulting .chm
-# file. You can add a path in front of the file if the result should not be
-# written to the html output directory.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-CHM_FILE               =
-
-# The HHC_LOCATION tag can be used to specify the location (absolute path
-# including file name) of the HTML help compiler (hhc.exe). If non-empty,
-# doxygen will try to run the HTML help compiler on the generated index.hhp.
-# The file has to be specified with full path.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-HHC_LOCATION           =
-
-# The GENERATE_CHI flag controls if a separate .chi index file is generated
-# (YES) or that it should be included in the master .chm file (NO).
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-GENERATE_CHI           = NO
-
-# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
-# and project file content.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-CHM_INDEX_ENCODING     =
-
-# The BINARY_TOC flag controls whether a binary table of contents is generated
-# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
-# enables the Previous and Next buttons.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-BINARY_TOC             = NO
-
-# The TOC_EXPAND flag can be set to YES to add extra items for group members to
-# the table of contents of the HTML help documentation and to the tree view.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
-
-TOC_EXPAND             = NO
-
-# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
-# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
-# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
-# (.qch) of the generated HTML documentation.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_QHP           = NO
-
-# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
-# the file name of the resulting .qch file. The path specified is relative to
-# the HTML output folder.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QCH_FILE               =
-
-# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
-# Project output. For more information please see Qt Help Project / Namespace
-# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_NAMESPACE          = org.doxygen.Project
-
-# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
-# Help Project output. For more information please see Qt Help Project / Virtual
-# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
-# folders).
-# The default value is: doc.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_VIRTUAL_FOLDER     = doc
-
-# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
-# filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
-# filters).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_CUST_FILTER_NAME   =
-
-# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
-# custom filter to add. For more information please see Qt Help Project / Custom
-# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
-# filters).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_CUST_FILTER_ATTRS  =
-
-# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
-# project's filter section matches. Qt Help Project / Filter Attributes (see:
-# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHP_SECT_FILTER_ATTRS  =
-
-# The QHG_LOCATION tag can be used to specify the location of Qt's
-# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
-# generated .qhp file.
-# This tag requires that the tag GENERATE_QHP is set to YES.
-
-QHG_LOCATION           =
-
-# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
-# generated, together with the HTML files, they form an Eclipse help plugin. To
-# install this plugin and make it available under the help contents menu in
-# Eclipse, the contents of the directory containing the HTML and XML files needs
-# to be copied into the plugins directory of eclipse. The name of the directory
-# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
-# After copying Eclipse needs to be restarted before the help appears.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_ECLIPSEHELP   = NO
-
-# A unique identifier for the Eclipse help plugin. When installing the plugin
-# the directory name containing the HTML and XML files should also have this
-# name. Each documentation set should have its own identifier.
-# The default value is: org.doxygen.Project.
-# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
-
-ECLIPSE_DOC_ID         = org.doxygen.Project
-
-# If you want full control over the layout of the generated HTML pages it might
-# be necessary to disable the index and replace it with your own. The
-# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
-# of each HTML page. A value of NO enables the index and the value YES disables
-# it. Since the tabs in the index contain the same information as the navigation
-# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-DISABLE_INDEX          = NO
-
-# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
-# structure should be generated to display hierarchical information. If the tag
-# value is set to YES, a side panel will be generated containing a tree-like
-# index structure (just like the one that is generated for HTML Help). For this
-# to work a browser that supports JavaScript, DHTML, CSS and frames is required
-# (i.e. any modern browser). Windows users are probably better off using the
-# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
-# further fine-tune the look of the index. As an example, the default style
-# sheet generated by doxygen has an example that shows how to put an image at
-# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
-# the same information as the tab index, you could consider setting
-# DISABLE_INDEX to YES when enabling this option.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-GENERATE_TREEVIEW      = NO
-
-# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
-# doxygen will group on one line in the generated HTML documentation.
-#
-# Note that a value of 0 will completely suppress the enum values from appearing
-# in the overview section.
-# Minimum value: 0, maximum value: 20, default value: 4.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-ENUM_VALUES_PER_LINE   = 4
-
-# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
-# to set the initial width (in pixels) of the frame in which the tree is shown.
-# Minimum value: 0, maximum value: 1500, default value: 250.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-TREEVIEW_WIDTH         = 250
-
-# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
-# external symbols imported via tag files in a separate window.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-EXT_LINKS_IN_WINDOW    = NO
-
-# Use this tag to change the font size of LaTeX formulas included as images in
-# the HTML documentation. When you change the font size after a successful
-# doxygen run you need to manually remove any form_*.png images from the HTML
-# output directory to force them to be regenerated.
-# Minimum value: 8, maximum value: 50, default value: 10.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-FORMULA_FONTSIZE       = 10
-
-# Use the FORMULA_TRANPARENT tag to determine whether or not the images
-# generated for formulas are transparent PNGs. Transparent PNGs are not
-# supported properly for IE 6.0, but are supported on all modern browsers.
-#
-# Note that when changing this option you need to delete any form_*.png files in
-# the HTML output directory before the changes have effect.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-FORMULA_TRANSPARENT    = YES
-
-# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
-# http://www.mathjax.org) which uses client side Javascript for the rendering
-# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
-# installed or if you want to formulas look prettier in the HTML output. When
-# enabled you may also need to install MathJax separately and configure the path
-# to it using the MATHJAX_RELPATH option.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-USE_MATHJAX            = NO
-
-# When MathJax is enabled you can set the default output format to be used for
-# the MathJax output. See the MathJax site (see:
-# http://docs.mathjax.org/en/latest/output.html) for more details.
-# Possible values are: HTML-CSS (which is slower, but has the best
-# compatibility), NativeMML (i.e. MathML) and SVG.
-# The default value is: HTML-CSS.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_FORMAT         = HTML-CSS
-
-# When MathJax is enabled you need to specify the location relative to the HTML
-# output directory using the MATHJAX_RELPATH option. The destination directory
-# should contain the MathJax.js script. For instance, if the mathjax directory
-# is located at the same level as the HTML output directory, then
-# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
-# Content Delivery Network so you can quickly see the result without installing
-# MathJax. However, it is strongly recommended to install a local copy of
-# MathJax from http://www.mathjax.org before deployment.
-# The default value is: http://cdn.mathjax.org/mathjax/latest.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
-
-# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
-# extension names that should be enabled during MathJax rendering. For example
-# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_EXTENSIONS     =
-
-# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
-# of code that will be used on startup of the MathJax code. See the MathJax site
-# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
-# example see the documentation.
-# This tag requires that the tag USE_MATHJAX is set to YES.
-
-MATHJAX_CODEFILE       =
-
-# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
-# the HTML output. The underlying search engine uses javascript and DHTML and
-# should work on any modern browser. Note that when using HTML help
-# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
-# there is already a search function so this one should typically be disabled.
-# For large projects the javascript based search engine can be slow, then
-# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
-# search using the keyboard; to jump to the search box use <access key> + S
-# (what the <access key> is depends on the OS and browser, but it is typically
-# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
-# key> to jump into the search results window, the results can be navigated
-# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
-# the search. The filter options can be selected when the cursor is inside the
-# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
-# to select a filter and <Enter> or <escape> to activate or cancel the filter
-# option.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_HTML is set to YES.
-
-SEARCHENGINE           = YES
-
-# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
-# implemented using a web server instead of a web client using Javascript. There
-# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
-# setting. When disabled, doxygen will generate a PHP script for searching and
-# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
-# and searching needs to be provided by external tools. See the section
-# "External Indexing and Searching" for details.
-# The default value is: NO.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SERVER_BASED_SEARCH    = NO
-
-# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
-# script for searching. Instead the search results are written to an XML file
-# which needs to be processed by an external indexer. Doxygen will invoke an
-# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
-# search results.
-#
-# Doxygen ships with an example indexer (doxyindexer) and search engine
-# (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: http://xapian.org/).
-#
-# See the section "External Indexing and Searching" for details.
-# The default value is: NO.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTERNAL_SEARCH        = NO
-
-# The SEARCHENGINE_URL should point to a search engine hosted by a web server
-# which will return the search results when EXTERNAL_SEARCH is enabled.
-#
-# Doxygen ships with an example indexer (doxyindexer) and search engine
-# (doxysearch.cgi) which are based on the open source search engine library
-# Xapian (see: http://xapian.org/). See the section "External Indexing and
-# Searching" for details.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SEARCHENGINE_URL       =
-
-# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
-# search data is written to a file for indexing by an external tool. With the
-# SEARCHDATA_FILE tag the name of this file can be specified.
-# The default file is: searchdata.xml.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-SEARCHDATA_FILE        = searchdata.xml
-
-# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
-# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
-# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
-# projects and redirect the results back to the right project.
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTERNAL_SEARCH_ID     =
-
-# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
-# projects other than the one defined by this configuration file, but that are
-# all added to the same external search index. Each project needs to have a
-# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
-# to a relative location where the documentation can be found. The format is:
-# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
-# This tag requires that the tag SEARCHENGINE is set to YES.
-
-EXTRA_SEARCH_MAPPINGS  =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the LaTeX output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
-# The default value is: YES.
-
-GENERATE_LATEX         = YES
-
-# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: latex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_OUTPUT           = latex
-
-# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
-# invoked.
-#
-# Note that when enabling USE_PDFLATEX this option is only used for generating
-# bitmaps for formulas in the HTML output, but not in the Makefile that is
-# written to the output directory.
-# The default file is: latex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_CMD_NAME         = latex
-
-# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
-# index for LaTeX.
-# The default file is: makeindex.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-MAKEINDEX_CMD_NAME     = makeindex
-
-# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
-# documents. This may be useful for small projects and may help to save some
-# trees in general.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-COMPACT_LATEX          = NO
-
-# The PAPER_TYPE tag can be used to set the paper type that is used by the
-# printer.
-# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
-# 14 inches) and executive (7.25 x 10.5 inches).
-# The default value is: a4.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-PAPER_TYPE             = a4
-
-# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
-# that should be included in the LaTeX output. The package can be specified just
-# by its name or with the correct syntax as to be used with the LaTeX
-# \usepackage command. To get the times font for instance you can specify :
-# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
-# To use the option intlimits with the amsmath package you can specify:
-# EXTRA_PACKAGES=[intlimits]{amsmath}
-# If left blank no extra packages will be included.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-EXTRA_PACKAGES         =
-
-# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
-# generated LaTeX document. The header should contain everything until the first
-# chapter. If it is left blank doxygen will generate a standard header. See
-# section "Doxygen usage" for information on how to let doxygen write the
-# default header to a separate file.
-#
-# Note: Only use a user-defined header if you know what you are doing! The
-# following commands have a special meaning inside the header: $title,
-# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
-# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
-# string, for the replacement values of the other commands the user is referred
-# to HTML_HEADER.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_HEADER           =
-
-# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
-# generated LaTeX document. The footer should contain everything after the last
-# chapter. If it is left blank doxygen will generate a standard footer. See
-# LATEX_HEADER for more information on how to generate a default footer and what
-# special commands can be used inside the footer.
-#
-# Note: Only use a user-defined footer if you know what you are doing!
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_FOOTER           =
-
-# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
-# LaTeX style sheets that are included after the standard style sheets created
-# by doxygen. Using this option one can overrule certain style aspects. Doxygen
-# will copy the style sheet files to the output directory.
-# Note: The order of the extra style sheet files is of importance (e.g. the last
-# style sheet in the list overrules the setting of the previous ones in the
-# list).
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_EXTRA_STYLESHEET =
-
-# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
-# other source files which should be copied to the LATEX_OUTPUT output
-# directory. Note that the files will be copied as-is; there are no commands or
-# markers available.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_EXTRA_FILES      =
-
-# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
-# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
-# contain links (just like the HTML output) instead of page references. This
-# makes the output suitable for online browsing using a PDF viewer.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-PDF_HYPERLINKS         = YES
-
-# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
-# the PDF file directly from the LaTeX files. Set this option to YES, to get a
-# higher quality PDF documentation.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-USE_PDFLATEX           = YES
-
-# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
-# command to the generated LaTeX files. This will instruct LaTeX to keep running
-# if errors occur, instead of asking the user for help. This option is also used
-# when generating formulas in HTML.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_BATCHMODE        = NO
-
-# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
-# index chapters (such as File Index, Compound Index, etc.) in the output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_HIDE_INDICES     = NO
-
-# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
-# code with syntax highlighting in the LaTeX output.
-#
-# Note that which sources are shown also depends on other settings such as
-# SOURCE_BROWSER.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_SOURCE_CODE      = NO
-
-# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
-# bibliography, e.g. plainnat, or ieeetr. See
-# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
-# The default value is: plain.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_BIB_STYLE        = plain
-
-# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
-# page will contain the date and time when the page was generated. Setting this
-# to NO can help when comparing the output of multiple runs.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_LATEX is set to YES.
-
-LATEX_TIMESTAMP        = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the RTF output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
-# RTF output is optimized for Word 97 and may not look too pretty with other RTF
-# readers/editors.
-# The default value is: NO.
-
-GENERATE_RTF           = NO
-
-# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: rtf.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_OUTPUT             = rtf
-
-# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
-# documents. This may be useful for small projects and may help to save some
-# trees in general.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-COMPACT_RTF            = NO
-
-# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
-# contain hyperlink fields. The RTF file will contain links (just like the HTML
-# output) instead of page references. This makes the output suitable for online
-# browsing using Word or some other Word compatible readers that support those
-# fields.
-#
-# Note: WordPad (write) and others do not support links.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_HYPERLINKS         = NO
-
-# Load stylesheet definitions from file. Syntax is similar to doxygen's config
-# file, i.e. a series of assignments. You only have to provide replacements,
-# missing definitions are set to their default value.
-#
-# See also section "Doxygen usage" for information on how to generate the
-# default style sheet that doxygen normally uses.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_STYLESHEET_FILE    =
-
-# Set optional variables used in the generation of an RTF document. Syntax is
-# similar to doxygen's config file. A template extensions file can be generated
-# using doxygen -e rtf extensionFile.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_EXTENSIONS_FILE    =
-
-# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
-# with syntax highlighting in the RTF output.
-#
-# Note that which sources are shown also depends on other settings such as
-# SOURCE_BROWSER.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_RTF is set to YES.
-
-RTF_SOURCE_CODE        = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the man page output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
-# classes and files.
-# The default value is: NO.
-
-GENERATE_MAN           = NO
-
-# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it. A directory man3 will be created inside the directory specified by
-# MAN_OUTPUT.
-# The default directory is: man.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_OUTPUT             = man
-
-# The MAN_EXTENSION tag determines the extension that is added to the generated
-# man pages. In case the manual section does not start with a number, the number
-# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
-# optional.
-# The default value is: .3.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_EXTENSION          = .3
-
-# The MAN_SUBDIR tag determines the name of the directory created within
-# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
-# MAN_EXTENSION with the initial . removed.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_SUBDIR             =
-
-# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
-# will generate one additional man file for each entity documented in the real
-# man page(s). These additional files only source the real man page, but without
-# them the man command would be unable to find the correct page.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_MAN is set to YES.
-
-MAN_LINKS              = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the XML output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
-# captures the structure of the code including all documentation.
-# The default value is: NO.
-
-GENERATE_XML           = NO
-
-# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
-# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
-# it.
-# The default directory is: xml.
-# This tag requires that the tag GENERATE_XML is set to YES.
-
-XML_OUTPUT             = xml
-
-# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
-# listings (including syntax highlighting and cross-referencing information) to
-# the XML output. Note that enabling this will significantly increase the size
-# of the XML output.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_XML is set to YES.
-
-XML_PROGRAMLISTING     = YES
-
-#---------------------------------------------------------------------------
-# Configuration options related to the DOCBOOK output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
-# that can be used to generate PDF.
-# The default value is: NO.
-
-GENERATE_DOCBOOK       = NO
-
-# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
-# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
-# front of it.
-# The default directory is: docbook.
-# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
-
-DOCBOOK_OUTPUT         = docbook
-
-# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
-# program listings (including syntax highlighting and cross-referencing
-# information) to the DOCBOOK output. Note that enabling this will significantly
-# increase the size of the DOCBOOK output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
-
-DOCBOOK_PROGRAMLISTING = NO
-
-#---------------------------------------------------------------------------
-# Configuration options for the AutoGen Definitions output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
-# AutoGen Definitions (see http://autogen.sf.net) file that captures the
-# structure of the code including all documentation. Note that this feature is
-# still experimental and incomplete at the moment.
-# The default value is: NO.
-
-GENERATE_AUTOGEN_DEF   = NO
-
-#---------------------------------------------------------------------------
-# Configuration options related to the Perl module output
-#---------------------------------------------------------------------------
-
-# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
-# file that captures the structure of the code including all documentation.
-#
-# Note that this feature is still experimental and incomplete at the moment.
-# The default value is: NO.
-
-GENERATE_PERLMOD       = NO
-
-# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
-# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
-# output from the Perl module output.
-# The default value is: NO.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_LATEX          = NO
-
-# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
-# formatted so it can be parsed by a human reader. This is useful if you want to
-# understand what is going on. On the other hand, if this tag is set to NO, the
-# size of the Perl module output will be much smaller and Perl will parse it
-# just the same.
-# The default value is: YES.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_PRETTY         = YES
-
-# The names of the make variables in the generated doxyrules.make file are
-# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
-# so different doxyrules.make files included by the same Makefile don't
-# overwrite each other's variables.
-# This tag requires that the tag GENERATE_PERLMOD is set to YES.
-
-PERLMOD_MAKEVAR_PREFIX =
-
-#---------------------------------------------------------------------------
-# Configuration options related to the preprocessor
-#---------------------------------------------------------------------------
-
-# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
-# C-preprocessor directives found in the sources and include files.
-# The default value is: YES.
-
-ENABLE_PREPROCESSING   = YES
-
-# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
-# in the source code. If set to NO, only conditional compilation will be
-# performed. Macro expansion can be done in a controlled way by setting
-# EXPAND_ONLY_PREDEF to YES.
-# The default value is: NO.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-MACRO_EXPANSION        = NO
-
-# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
-# the macro expansion is limited to the macros specified with the PREDEFINED and
-# EXPAND_AS_DEFINED tags.
-# The default value is: NO.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-EXPAND_ONLY_PREDEF     = NO
-
-# If the SEARCH_INCLUDES tag is set to YES, the include files in the
-# INCLUDE_PATH will be searched if a #include is found.
-# The default value is: YES.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-SEARCH_INCLUDES        = YES
-
-# The INCLUDE_PATH tag can be used to specify one or more directories that
-# contain include files that are not input files but should be processed by the
-# preprocessor.
-# This tag requires that the tag SEARCH_INCLUDES is set to YES.
-
-INCLUDE_PATH           =
-
-# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
-# patterns (like *.h and *.hpp) to filter out the header-files in the
-# directories. If left blank, the patterns specified with FILE_PATTERNS will be
-# used.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-INCLUDE_FILE_PATTERNS  =
-
-# The PREDEFINED tag can be used to specify one or more macro names that are
-# defined before the preprocessor is started (similar to the -D option of e.g.
-# gcc). The argument of the tag is a list of macros of the form: name or
-# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
-# is assumed. To prevent a macro definition from being undefined via #undef or
-# recursively expanded use the := operator instead of the = operator.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-PREDEFINED             =
-
-# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
-# tag can be used to specify a list of macro names that should be expanded. The
-# macro definition that is found in the sources will be used. Use the PREDEFINED
-# tag if you want to use a different macro definition that overrules the
-# definition found in the source code.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-EXPAND_AS_DEFINED      =
-
-# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
-# remove all references to function-like macros that are alone on a line, have
-# an all uppercase name, and do not end with a semicolon. Such function macros
-# are typically used for boiler-plate code, and will confuse the parser if not
-# removed.
-# The default value is: YES.
-# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
-
-SKIP_FUNCTION_MACROS   = YES
-
-#---------------------------------------------------------------------------
-# Configuration options related to external references
-#---------------------------------------------------------------------------
-
-# The TAGFILES tag can be used to specify one or more tag files. For each tag
-# file the location of the external documentation should be added. The format of
-# a tag file without this location is as follows:
-# TAGFILES = file1 file2 ...
-# Adding location for the tag files is done as follows:
-# TAGFILES = file1=loc1 "file2 = loc2" ...
-# where loc1 and loc2 can be relative or absolute paths or URLs. See the
-# section "Linking to external documentation" for more information about the use
-# of tag files.
-# Note: Each tag file must have a unique name (where the name does NOT include
-# the path). If a tag file is not located in the directory in which doxygen is
-# run, you must also specify the path to the tagfile here.
-
-TAGFILES               =
-
-# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
-# tag file that is based on the input files it reads. See section "Linking to
-# external documentation" for more information about the usage of tag files.
-
-GENERATE_TAGFILE       =
-
-# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
-# the class index. If set to NO, only the inherited external classes will be
-# listed.
-# The default value is: NO.
-
-ALLEXTERNALS           = NO
-
-# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
-# in the modules index. If set to NO, only the current project's groups will be
-# listed.
-# The default value is: YES.
-
-EXTERNAL_GROUPS        = YES
-
-# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
-# the related pages index. If set to NO, only the current project's pages will
-# be listed.
-# The default value is: YES.
-
-EXTERNAL_PAGES         = YES
-
-# The PERL_PATH should be the absolute path and name of the perl script
-# interpreter (i.e. the result of 'which perl').
-# The default file (with absolute path) is: /usr/bin/perl.
-
-PERL_PATH              = /usr/bin/perl
-
-#---------------------------------------------------------------------------
-# Configuration options related to the dot tool
-#---------------------------------------------------------------------------
-
-# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
-# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
-# NO turns the diagrams off. Note that this option also works with HAVE_DOT
-# disabled, but it is recommended to install and use dot, since it yields more
-# powerful graphs.
-# The default value is: YES.
-
-CLASS_DIAGRAMS         = YES
-
-# You can define message sequence charts within doxygen comments using the \msc
-# command. Doxygen will then run the mscgen tool (see:
-# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
-# documentation. The MSCGEN_PATH tag allows you to specify the directory where
-# the mscgen tool resides. If left empty the tool is assumed to be found in the
-# default search path.
-
-MSCGEN_PATH            =
-
-# You can include diagrams made with dia in doxygen documentation. Doxygen will
-# then run dia to produce the diagram and insert it in the documentation. The
-# DIA_PATH tag allows you to specify the directory where the dia binary resides.
-# If left empty dia is assumed to be found in the default search path.
-
-DIA_PATH               =
-
-# If set to YES the inheritance and collaboration graphs will hide inheritance
-# and usage relations if the target is undocumented or is not a class.
-# The default value is: YES.
-
-HIDE_UNDOC_RELATIONS   = YES
-
-# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
-# available from the path. This tool is part of Graphviz (see:
-# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
-# Bell Labs. The other options in this section have no effect if this option is
-# set to NO
-# The default value is: YES.
-
-HAVE_DOT               = YES
-
-# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
-# to run in parallel. When set to 0 doxygen will base this on the number of
-# processors available in the system. You can set it explicitly to a value
-# larger than 0 to get control over the balance between CPU load and processing
-# speed.
-# Minimum value: 0, maximum value: 32, default value: 0.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_NUM_THREADS        = 0
-
-# When you want a differently looking font in the dot files that doxygen
-# generates you can specify the font name using DOT_FONTNAME. You need to make
-# sure dot is able to find the font, which can be done by putting it in a
-# standard location or by setting the DOTFONTPATH environment variable or by
-# setting DOT_FONTPATH to the directory containing the font.
-# The default value is: Helvetica.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTNAME           = Helvetica
-
-# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
-# dot graphs.
-# Minimum value: 4, maximum value: 24, default value: 10.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTSIZE           = 10
-
-# By default doxygen will tell dot to use the default font as specified with
-# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
-# the path where dot can find it using this tag.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_FONTPATH           =
-
-# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
-# each documented class showing the direct and indirect inheritance relations.
-# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CLASS_GRAPH            = YES
-
-# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
-# graph for each documented class showing the direct and indirect implementation
-# dependencies (inheritance, containment, and class references variables) of the
-# class with other documented classes.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-COLLABORATION_GRAPH    = YES
-
-# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
-# groups, showing the direct groups dependencies.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GROUP_GRAPHS           = YES
-
-# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
-# collaboration diagrams in a style similar to the OMG's Unified Modeling
-# Language.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-UML_LOOK               = NO
-
-# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
-# class node. If there are many fields or methods and many nodes the graph may
-# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
-# number of items for each type to make the size more manageable. Set this to 0
-# for no limit. Note that the threshold may be exceeded by 50% before the limit
-# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
-# but if the number exceeds 15, the total amount of fields shown is limited to
-# 10.
-# Minimum value: 0, maximum value: 100, default value: 10.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-UML_LIMIT_NUM_FIELDS   = 10
-
-# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
-# collaboration graphs will show the relations between templates and their
-# instances.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-TEMPLATE_RELATIONS     = NO
-
-# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
-# YES then doxygen will generate a graph for each documented file showing the
-# direct and indirect include dependencies of the file with other documented
-# files.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INCLUDE_GRAPH          = YES
-
-# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
-# set to YES then doxygen will generate a graph for each documented file showing
-# the direct and indirect include dependencies of the file with other documented
-# files.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INCLUDED_BY_GRAPH      = YES
-
-# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
-# dependency graph for every global function or class method.
-#
-# Note that enabling this option will significantly increase the time of a run.
-# So in most cases it will be better to enable call graphs for selected
-# functions only using the \callgraph command. Disabling a call graph can be
-# accomplished by means of the command \hidecallgraph.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CALL_GRAPH             = YES
-
-# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
-# dependency graph for every global function or class method.
-#
-# Note that enabling this option will significantly increase the time of a run.
-# So in most cases it will be better to enable caller graphs for selected
-# functions only using the \callergraph command. Disabling a caller graph can be
-# accomplished by means of the command \hidecallergraph.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-CALLER_GRAPH           = YES
-
-# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
-# hierarchy of all classes instead of a textual one.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GRAPHICAL_HIERARCHY    = YES
-
-# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
-# dependencies a directory has on other directories in a graphical way. The
-# dependency relations are determined by the #include relations between the
-# files in the directories.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DIRECTORY_GRAPH        = YES
-
-# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
-# generated by dot. For an explanation of the image formats see the section
-# output formats in the documentation of the dot tool (Graphviz (see:
-# http://www.graphviz.org/)).
-# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
-# to make the SVG files visible in IE 9+ (other browsers do not have this
-# requirement).
-# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
-# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
-# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
-# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
-# png:gdiplus:gdiplus.
-# The default value is: png.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_IMAGE_FORMAT       = png
-
-# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
-# enable generation of interactive SVG images that allow zooming and panning.
-#
-# Note that this requires a modern browser other than Internet Explorer. Tested
-# and working are Firefox, Chrome, Safari, and Opera.
-# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
-# the SVG files visible. Older versions of IE do not have SVG support.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-INTERACTIVE_SVG        = NO
-
-# The DOT_PATH tag can be used to specify the path where the dot tool can be
-# found. If left blank, it is assumed the dot tool can be found in the path.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_PATH               =
-
-# The DOTFILE_DIRS tag can be used to specify one or more directories that
-# contain dot files that are included in the documentation (see the \dotfile
-# command).
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOTFILE_DIRS           =
-
-# The MSCFILE_DIRS tag can be used to specify one or more directories that
-# contain msc files that are included in the documentation (see the \mscfile
-# command).
-
-MSCFILE_DIRS           =
-
-# The DIAFILE_DIRS tag can be used to specify one or more directories that
-# contain dia files that are included in the documentation (see the \diafile
-# command).
-
-DIAFILE_DIRS           =
-
-# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
-# path where java can find the plantuml.jar file. If left blank, it is assumed
-# PlantUML is not used or called during a preprocessing step. Doxygen will
-# generate a warning when it encounters a \startuml command in this case and
-# will not generate output for the diagram.
-
-PLANTUML_JAR_PATH      =
-
-# When using plantuml, the specified paths are searched for files specified by
-# the !include statement in a plantuml block.
-
-PLANTUML_INCLUDE_PATH  =
-
-# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
-# that will be shown in the graph. If the number of nodes in a graph becomes
-# larger than this value, doxygen will truncate the graph, which is visualized
-# by representing a node as a red box. Note that doxygen if the number of direct
-# children of the root node in a graph is already larger than
-# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
-# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
-# Minimum value: 0, maximum value: 10000, default value: 50.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_GRAPH_MAX_NODES    = 50
-
-# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
-# generated by dot. A depth value of 3 means that only nodes reachable from the
-# root by following a path via at most 3 edges will be shown. Nodes that lay
-# further from the root node will be omitted. Note that setting this option to 1
-# or 2 may greatly reduce the computation time needed for large code bases. Also
-# note that the size of a graph can be further restricted by
-# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
-# Minimum value: 0, maximum value: 1000, default value: 0.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-MAX_DOT_GRAPH_DEPTH    = 0
-
-# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
-# background. This is disabled by default, because dot on Windows does not seem
-# to support this out of the box.
-#
-# Warning: Depending on the platform used, enabling this option may lead to
-# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
-# read).
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_TRANSPARENT        = NO
-
-# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
-# files in one run (i.e. multiple -o and -T options on the command line). This
-# makes dot run faster, but since only newer versions of dot (>1.8.10) support
-# this, this feature is disabled by default.
-# The default value is: NO.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_MULTI_TARGETS      = NO
-
-# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
-# explaining the meaning of the various boxes and arrows in the dot generated
-# graphs.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-GENERATE_LEGEND        = YES
-
-# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
-# files that are used to generate the various graphs.
-# The default value is: YES.
-# This tag requires that the tag HAVE_DOT is set to YES.
-
-DOT_CLEANUP            = YES
diff --git a/fixposition_driver_ros2/README.md b/fixposition_driver_ros2/README.md
index eaf1734..554164b 100644
--- a/fixposition_driver_ros2/README.md
+++ b/fixposition_driver_ros2/README.md
@@ -1,21 +1,5 @@
 # Fixposition ROS2 Driver
 
--   [ROS2 Humble / Jazzy ![](./../../actions/workflows/build_test_ros2.yml/badge.svg)](./../../actions/workflows/build_test_ros2.yml)
-
 [ROS](https://www.ros.org/) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product).
 
-## Driver documentation
-
-For installation, usage, and more information, please refer to [Fixposition Docs: ROS Driver](https://docs.fixposition.com/fd/fixposition-ros-driver).
-
-## Fixposition ASCII messages
-
-For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs: I/O messages](https://docs.fixposition.com/fd/i-o-messages).
-
-## Code Documentation
-
-Run `doxygen Doxyfile` to generate Doxygen code documentation.
-
-## License
-
-This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details
+See the [main README.md](../README.md) for the documentation.
diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
index 31959e8..c8c2425 100644
--- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
+++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
@@ -1,28 +1,113 @@
 /**
- *  @file
- *  @brief Convert Data classes to ROS2 msgs
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Convert data to ROS2 msgs
  */
 
-#ifndef __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2__
-#define __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2__
+#ifndef __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__
+#define __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__
+
+/* LIBC/STL */
+#include <memory>
+#include <unordered_map>
 
-/* FIXPOSITION DRIVER LIB */
-#include <fixposition_driver_lib/messages/msg_data.hpp>
+/* EXTERNAL */
 #include <fixposition_driver_lib/fixposition_driver.hpp>
+#include <fixposition_driver_lib/helper.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_ros2/ext/rclcpp.hpp>
 
 /* PACKAGE */
-#include <fixposition_driver_ros2/fixposition_driver_node.hpp>
+#include "ros2_msgs.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
+
+void TfDataToTransformStamped(const TfData& data, geometry_msgs::msg::TransformStamped& msg);
+void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::msg::TransformStamped& msg);
+
+void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload,
+                        rclcpp::Publisher<fpmsgs::FpaOdometry>::SharedPtr& pub);
+void PublishFpaOdometryDataImu(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload,
+                               rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub);
+void PublishFpaOdometryDataNavSatFix(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload,
+                                     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub);
+void PublishFpaOdomenu(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload,
+                       rclcpp::Publisher<fpmsgs::FpaOdomenu>::SharedPtr& pub);
+void PublishFpaOdomenuVector3Stamped(const fpsdk::common::parser::fpa::FpaOdomenuPayload& payload,
+                                     rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr& pub);
+void PublishFpaOdomsh(const fpsdk::common::parser::fpa::FpaOdomshPayload& payload,
+                      rclcpp::Publisher<fpmsgs::FpaOdomsh>::SharedPtr& pub);
+void PublishFpaOdomstatus(const fpsdk::common::parser::fpa::FpaOdomstatusPayload& payload,
+                          rclcpp::Publisher<fpmsgs::FpaOdomstatus>::SharedPtr& pub);
+void PublishFpaLlh(const fpsdk::common::parser::fpa::FpaLlhPayload& payload,
+                   rclcpp::Publisher<fpmsgs::FpaLlh>::SharedPtr& pub);
+void PublishFpaEoe(const fpsdk::common::parser::fpa::FpaEoePayload& payload,
+                   rclcpp::Publisher<fpmsgs::FpaEoe>::SharedPtr& pub);
+void PublishFpaImubias(const fpsdk::common::parser::fpa::FpaImubiasPayload& payload,
+                       rclcpp::Publisher<fpmsgs::FpaImubias>::SharedPtr& pub);
+void PublishFpaGnssant(const fpsdk::common::parser::fpa::FpaGnssantPayload& payload,
+                       rclcpp::Publisher<fpmsgs::FpaGnssant>::SharedPtr& pub);
+void PublishFpaGnsscorr(const fpsdk::common::parser::fpa::FpaGnsscorrPayload& payload,
+                        rclcpp::Publisher<fpmsgs::FpaGnsscorr>::SharedPtr& pub);
+void PublishFpaTp(const fpsdk::common::parser::fpa::FpaTpPayload& payload,
+                  rclcpp::Publisher<fpmsgs::FpaTp>::SharedPtr& pub);
+void PublishFpaText(const fpsdk::common::parser::fpa::FpaTextPayload& payload,
+                    rclcpp::Publisher<fpmsgs::FpaText>::SharedPtr& pub);
+void PublishFpaRawimu(const fpsdk::common::parser::fpa::FpaRawimuPayload& payload,
+                      rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub);
+void PublishFpaCorrimu(const fpsdk::common::parser::fpa::FpaCorrimuPayload& payload,
+                       rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub);
+
+bool PublishNovbBestgnsspos(const fpsdk::common::parser::novb::NovbHeader* header,
+                            const fpsdk::common::parser::novb::NovbBestgnsspos* payload,
+                            rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub1,
+                            rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub2);
+
+void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr& pub);
+void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGll>::SharedPtr& pub);
+void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGsa>::SharedPtr& pub);
+void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGst>::SharedPtr& pub);
+void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGsv>::SharedPtr& pub);
+void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaHdt>::SharedPtr& pub);
+void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaRmc>::SharedPtr& pub);
+void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaVtg>::SharedPtr& pub);
+void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaZda>::SharedPtr& pub);
+
+void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg,
+                      rclcpp::Publisher<fpmsgs::ParserMsg>::SharedPtr& pub);
+
+void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher<fpmsgs::NmeaEpoch>::SharedPtr& pub);
+void PublishOdometryData(const OdometryData& data, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr& pub);
+void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher<fpmsgs::CovWarn>::SharedPtr& pub);
+
+#if 0
+inline void PublishFpaOdometry(const fpsdk::common::parser::fpa::FpaOdometryPayload& payload,
+                               rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMETRY>::SharedPtr& pub) {
+    (void)payload;
+    if (pub->get_subscription_count() > 0) {
+        fixposition_driver_ros2::msg::ODOMETRY msg;
+        msg.header.stamp = rclcpp::Clock().now();
+        pub->publish(msg);
+    }
+}
 
 /**
  * @brief Convert to ROS2 message time
@@ -51,19 +136,19 @@ inline builtin_interfaces::msg::Time GpsTimeToMsgTime(times::GpsTime input) {
  * @param[in] data
  * @param[out] msg
  */
-void FpToRosMsg( const OdometryData& data, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub);
-void FpToRosMsg(      const ImuData& data, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub);
-void FpToRosMsg(       const FP_EOE& data, rclcpp::Publisher<fixposition_driver_ros2::msg::EOE>::SharedPtr pub);
-void FpToRosMsg(   const FP_GNSSANT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSANT>::SharedPtr pub);
-void FpToRosMsg(  const FP_GNSSCORR& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSCORR>::SharedPtr pub);
-void FpToRosMsg(   const FP_IMUBIAS& data, rclcpp::Publisher<fixposition_driver_ros2::msg::IMUBIAS>::SharedPtr pub);
-void FpToRosMsg(       const FP_LLH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::LLH>::SharedPtr pub);
-void FpToRosMsg(   const FP_ODOMENU& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMENU>::SharedPtr pub);
-void FpToRosMsg(  const FP_ODOMETRY& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMETRY>::SharedPtr pub);
-void FpToRosMsg(    const FP_ODOMSH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSH>::SharedPtr pub);
+void FpToRosMsg(const OdometryData& data, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub);
+void FpToRosMsg(const ImuData& data, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub);
+void FpToRosMsg(const FP_EOE& data, rclcpp::Publisher<fixposition_driver_ros2::msg::EOE>::SharedPtr pub);
+void FpToRosMsg(const FP_GNSSANT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSANT>::SharedPtr pub);
+void FpToRosMsg(const FP_GNSSCORR& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSCORR>::SharedPtr pub);
+void FpToRosMsg(const FP_IMUBIAS& data, rclcpp::Publisher<fixposition_driver_ros2::msg::IMUBIAS>::SharedPtr pub);
+void FpToRosMsg(const FP_LLH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::LLH>::SharedPtr pub);
+void FpToRosMsg(const FP_ODOMENU& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMENU>::SharedPtr pub);
+void FpToRosMsg(const FP_ODOMETRY& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMETRY>::SharedPtr pub);
+void FpToRosMsg(const FP_ODOMSH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSH>::SharedPtr pub);
 void FpToRosMsg(const FP_ODOMSTATUS& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSTATUS>::SharedPtr pub);
-void FpToRosMsg(        const FP_TP& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TP>::SharedPtr pub);
-void FpToRosMsg(      const FP_TEXT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TEXT>::SharedPtr pub);
+void FpToRosMsg(const FP_TP& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TP>::SharedPtr pub);
+void FpToRosMsg(const FP_TEXT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TEXT>::SharedPtr pub);
 
 void FpToRosMsg(const GP_GGA& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPGGA>::SharedPtr pub);
 void FpToRosMsg(const GP_GLL& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPGLL>::SharedPtr pub);
@@ -130,7 +215,9 @@ void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf
  * @param[out] static_br_
  * @param[out] br_
  */
-void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::msg::TransformStamped>>& tf_map, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_, std::shared_ptr<tf2_ros::TransformBroadcaster> br_);
+void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::msg::TransformStamped>>& tf_map,
+                   std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_,
+                   std::shared_ptr<tf2_ros::TransformBroadcaster> br_);
 
 /**
  * @brief
@@ -138,7 +225,8 @@ void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::ms
  * @param[in] data
  * @param[out] msg
  */
-void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr pub);
+void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data,
+                     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr pub);
 
 /**
  * @brief
@@ -164,9 +252,12 @@ void OdomToYprMsg(const OdometryData& data, rclcpp::Publisher<geometry_msgs::msg
  * @param[in] prev_cov
  * @param[out] msg
  */
-void JumpWarningMsg(std::shared_ptr<rclcpp::Node> node, const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, 
-                    const Eigen::MatrixXd& prev_cov, rclcpp::Publisher<fixposition_driver_ros2::msg::COVWARN>::SharedPtr pub);
-
-}  // namespace fixposition
+void JumpWarningMsg(std::shared_ptr<rclcpp::Node> node, const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff,
+                    const Eigen::MatrixXd& prev_cov,
+                    rclcpp::Publisher<fixposition_driver_ros2::msg::COVWARN>::SharedPtr pub);
 
 #endif
+
+/* ****************************************************************************************************************** */
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_ROS2_DATA_TO_ROS2_HPP__
diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
index adf480a..3979f15 100644
--- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
+++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
@@ -1,141 +1,146 @@
 /**
- *  @file
- *  @brief Declaration of FixpositionDriver ROS2 Node
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Fixposition driver node for ROS2
  */
 
-#ifndef __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_
-#define __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_
-
-/* SYSTEM / STL */
-#include <termios.h>
+#ifndef __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_HPP__
+#define __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_HPP__
 
-/* ROS2 */
-#include <fixposition_driver_ros2/ros2_msgs.hpp>
-#include <fixposition_driver_ros2/params.hpp>
+/* LIBC/STL */
+#include <memory>
+#include <mutex>
 
-/* FIXPOSITION */
+/* EXTERNAL */
 #include <fixposition_driver_lib/helper.hpp>
-#include <fixposition_driver_lib/gnss_tf.hpp>
+#include <fpsdk_ros2/ext/rclcpp.hpp>
 
 /* PACKAGE */
-#include <fixposition_driver_ros2/data_to_ros2.hpp>
+#include "data_to_ros2.hpp"
+#include "params.hpp"
+#include "ros2_msgs.hpp"
 
 namespace fixposition {
-class FixpositionDriverNode : public FixpositionDriver {
+/* ****************************************************************************************************************** */
+
+using namespace fpsdk::common;
+
+class FixpositionDriverNode {
    public:
     /**
-     * @brief Construct a new Fixposition Driver Node object
+     * @brief Constructor
      *
-     * @param[in] params
+     * @param[in]  nh             Node handle
+     * @param[in]  driver_params  Parameters
+     * @param[in]  node_params    Parameters
      */
-    FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node, const FixpositionDriverParams& params, rclcpp::QoS qos_settings);
-
-    void Run();
+    FixpositionDriverNode(std::shared_ptr<rclcpp::Node> nh, const DriverParams& driver_params,
+                          const NodeParams& node_params);
 
-    void RegisterObservers();
-
-    void WsCallbackRos(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg);
-
-    void RtcmCallbackRos(const rtcm_msgs::msg::Message::ConstSharedPtr msg);
+    /**
+     * @brief Destructor
+     */
+    ~FixpositionDriverNode();
 
-   private:
     /**
-     * @brief Observer Functions to publish NavSatFix from BestGnssPos
+     * @brief Start the node
      *
-     * @param[in] header
-     * @param[in] payload
+     * @returns true on success, false otherwise
      */
-    void BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, const BESTGNSSPOSMem* payload);
+    bool StartNode();
 
     /**
-     * @brief Observer Function to publish NMEA message once the GNSS epoch transmission is complete
-     *
-     * @param[in] data
+     * @brief Stop the node
      */
-    void PublishNmea();
-    
-    // ROS node handler
-    std::shared_ptr<rclcpp::Node> node_;
+    void StopNode();
 
-    // ROS subscribers
-    rclcpp::Subscription<fixposition_driver_ros2::msg::Speed>::SharedPtr ws_sub_;  //!< wheelspeed message subscriber
-    rclcpp::Subscription<rtcm_msgs::msg::Message>::SharedPtr rtcm_sub_;            //!< RTCM3 message subscriber
+   private:
+    std::shared_ptr<rclcpp::Node> nh_;  //!< Node handle
+    DriverParams driver_params_;        //!< Sensor/driver parameters
+    NodeParams node_params_;            //!< Node parameters
+    rclcpp::Logger logger_;             //!< Logger
+    FixpositionDriver driver_;          //!< Sensor driver
+    rclcpp::QoS qos_settings_;          //!< QoS settings
 
     // ROS publishers
-    // FP_A messages
-    rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMETRY>::SharedPtr fpa_odometry_pub_;      //!< FP_A-ODOMETRY message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::IMUBIAS>::SharedPtr fpa_imubias_pub_;        //!< FP_A-IMUBIAS message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::EOE>::SharedPtr fpa_eoe_pub_;                //!< FP_A-EOE message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::LLH>::SharedPtr fpa_llh_pub_;                //!< FP_A-LLH message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMENU>::SharedPtr fpa_odomenu_pub_;        //!< FP_A-ODOMENU message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSH>::SharedPtr fpa_odomsh_pub_;          //!< FP_A-ODOMSH message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSTATUS>::SharedPtr fpa_odomstatus_pub_;  //!< FP_A-ODOMSTATUS message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSANT>::SharedPtr fpa_gnssant_pub_;        //!< FP_A-GNSSANT message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSCORR>::SharedPtr fpa_gnsscorr_pub_;      //!< FP_A-GNSSCORR message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::TEXT>::SharedPtr fpa_text_pub_;              //!< FP_A-TEXT message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::TP>::SharedPtr fpa_tp_pub_;                  //!< FP_A-TP message
-
-    // NMEA messages
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GPGGA>::SharedPtr nmea_gpgga_pub_;           //!< NMEA-GP-GGA message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GPGLL>::SharedPtr nmea_gpgll_pub_;           //!< NMEA-GP-GLL message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GNGSA>::SharedPtr nmea_gngsa_pub_;           //!< NMEA-GP-GSA message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GPGST>::SharedPtr nmea_gpgst_pub_;           //!< NMEA-GP-GST message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GXGSV>::SharedPtr nmea_gxgsv_pub_;           //!< NMEA-GP-GSV message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GPHDT>::SharedPtr nmea_gphdt_pub_;           //!< NMEA-GP-HDT message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GPRMC>::SharedPtr nmea_gprmc_pub_;           //!< NMEA-GP-RMC message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GPVTG>::SharedPtr nmea_gpvtg_pub_;           //!< NMEA-GP-VTG message
-    rclcpp::Publisher<fixposition_driver_ros2::msg::GPZDA>::SharedPtr nmea_gpzda_pub_;           //!< NMEA-GP-ZDA message
-
-    // ODOMETRY
-    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_ecef_pub_;    //!< ECEF Odometry
-    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr odometry_llh_pub_; //!< LLH Odometry
-    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu_pub_;     //!< ENU Odometry
-    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_smooth_pub_;  //!< Smooth Odometry (ECEF)
-
-    // Orientation
-    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_pub_;              //!< Euler angles Yaw-Pitch-Roll in local ENU
-    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_imu_pub_;          //!< Euler angles Pitch-Roll as estimated from the IMU in local horizontal
-
-    // IMU
-    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr rawimu_pub_;                        //!< Raw IMU data in IMU frame
-    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr corrimu_pub_;                       //!< Bias corrected IMU data in IMU frame
-    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;                        //!< Bias corrected IMU data in POI frame
-
-    // GNSS
-    rclcpp::Publisher<fixposition_driver_ros2::msg::NMEA>::SharedPtr nmea_pub_;             //!< Pose estimation based only on GNSS
-    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss1_pub_;         //!< GNSS1 position and status
-    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss2_pub_;         //!< GNSS2 position and status
-    NmeaMessage nmea_message_;                                                              //!< Collector class for NMEA messages
-    
-    // TF
-    std::shared_ptr<tf2_ros::TransformBroadcaster> br_;
-    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_;
-
-    // Jump warning topic
-    rclcpp::Publisher<fixposition_driver_ros2::msg::COVWARN>::SharedPtr extras_jump_pub_;   //!< Jump warning topic
-
-    // Previous state
-    Eigen::Vector3d prev_pos;
-    Eigen::MatrixXd prev_cov;
-
-    // Nav2 TF map
-    std::map<std::string, std::shared_ptr<geometry_msgs::msg::TransformStamped>> tf_map = {
-        {"ECEFENU0", nullptr},
-        {"POIPOISH", nullptr},
-        {"ECEFPOISH", nullptr},
-        {"ENU0POI", nullptr}
+    // - FP_A messages
+    rclcpp::Publisher<fpmsgs::FpaEoe>::SharedPtr fpa_eoe_pub_;                //!< FP_A-EOE message
+    rclcpp::Publisher<fpmsgs::FpaGnssant>::SharedPtr fpa_gnssant_pub_;        //!< FP_A-GNSSANT message
+    rclcpp::Publisher<fpmsgs::FpaGnsscorr>::SharedPtr fpa_gnsscorr_pub_;      //!< FP_A-GNSSCORR message
+    rclcpp::Publisher<fpmsgs::FpaImubias>::SharedPtr fpa_imubias_pub_;        //!< FP_A-IMUBIAS message
+    rclcpp::Publisher<fpmsgs::FpaLlh>::SharedPtr fpa_llh_pub_;                //!< FP_A-LLH message
+    rclcpp::Publisher<fpmsgs::FpaOdomenu>::SharedPtr fpa_odomenu_pub_;        //!< FP_A-ODOMENU message
+    rclcpp::Publisher<fpmsgs::FpaOdometry>::SharedPtr fpa_odometry_pub_;      //!< FP_A-ODOMETRY message
+    rclcpp::Publisher<fpmsgs::FpaOdomsh>::SharedPtr fpa_odomsh_pub_;          //!< FP_A-ODOMSH message
+    rclcpp::Publisher<fpmsgs::FpaOdomstatus>::SharedPtr fpa_odomstatus_pub_;  //!< FP_A-ODOMSTATUS message
+    rclcpp::Publisher<fpmsgs::FpaText>::SharedPtr fpa_text_pub_;              //!< FP_A-TEXT message
+    rclcpp::Publisher<fpmsgs::FpaTp>::SharedPtr fpa_tp_pub_;                  //!< FP_A-TP message
+    // - NMEA messages
+    rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr nmea_gga_pub_;  //!< NMEA-GP-GGA message
+    rclcpp::Publisher<fpmsgs::NmeaGll>::SharedPtr nmea_gll_pub_;  //!< NMEA-GP-GLL message
+    rclcpp::Publisher<fpmsgs::NmeaGsa>::SharedPtr nmea_gsa_pub_;  //!< NMEA-GN-GSA message
+    rclcpp::Publisher<fpmsgs::NmeaGst>::SharedPtr nmea_gst_pub_;  //!< NMEA-GP-GST message
+    rclcpp::Publisher<fpmsgs::NmeaGsv>::SharedPtr nmea_gsv_pub_;  //!< NMEA-GX-GSV message
+    rclcpp::Publisher<fpmsgs::NmeaHdt>::SharedPtr nmea_hdt_pub_;  //!< NMEA-GP-HDT message
+    rclcpp::Publisher<fpmsgs::NmeaRmc>::SharedPtr nmea_rmc_pub_;  //!< NMEA-GP-RMC message
+    rclcpp::Publisher<fpmsgs::NmeaVtg>::SharedPtr nmea_vtg_pub_;  //!< NMEA-GP-VTG message
+    rclcpp::Publisher<fpmsgs::NmeaZda>::SharedPtr nmea_zda_pub_;  //!< NMEA-GP-ZDA message
+    // - Odometry
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_ecef_pub_;     //!< ECEF odometry
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu_pub_;      //!< ENU odometry
+    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_smooth_pub_;   //!< Smooth odometry (ECEF)
+    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr odometry_llh_pub_;  //!< LLH odometry
+    // - Orientation
+    //! Euler angles yaw-pitch-roll in local ENU
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_pub_;
+    //! Euler angles pitch-roll as estimated from the IMU in local horizontal
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_imu_pub_;
+    // - IMU
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr rawimu_pub_;   //!< Raw IMU data in IMU frame
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr corrimu_pub_;  //!< Bias corrected IMU data in IMU frame
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;   //!< Bias corrected IMU data in POI frame
+    // - GNSS
+    rclcpp::Publisher<fpmsgs::NmeaEpoch>::SharedPtr nmea_epoch_pub_;                 //!< NMEA epoch data
+    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss1_pub_;  //!< GNSS1 position and status
+    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsatfix_gnss2_pub_;  //!< GNSS2 position and status
+    // - Other
+    rclcpp::Publisher<fpmsgs::CovWarn>::SharedPtr jump_pub_;   //!< Jump warning topic
+    rclcpp::Publisher<fpmsgs::ParserMsg>::SharedPtr raw_pub_;  //!< Raw messages topic
+
+    // ROS subscribers
+    rclcpp::Subscription<fpmsgs::Speed>::SharedPtr ws_sub_;              //!< Wheelspeed input subscriber
+    rclcpp::Subscription<rtcm_msgs::msg::Message>::SharedPtr corr_sub_;  //!< GNSS correction data input subscriber
+
+    // TF broadcasters
+    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_br_;
+    std::unique_ptr<tf2_ros::StaticTransformBroadcaster> static_br_;
+
+    // State
+    JumpDetector jump_detector_;
+    NmeaEpochData nmea_epoch_data_;  //!< NMEA collector
+
+    // TFs
+    struct Tfs {
+        std::mutex mutex_;
+        std::unique_ptr<geometry_msgs::msg::TransformStamped> ecef_enu0_;
+        std::unique_ptr<geometry_msgs::msg::TransformStamped> poi_poish_;
+        std::unique_ptr<geometry_msgs::msg::TransformStamped> ecef_poish_;
+        std::unique_ptr<geometry_msgs::msg::TransformStamped> enu0_poi_;
     };
+    Tfs tfs_;
+
+    void ProcessTfData(const TfData& tf_data);
+    void ProcessOdometryData(const OdometryData& odometry_data);
+    void PublishNav2Tf();
 };
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-
-#endif
+#endif  // __FIXPOSITION_DRIVER_ROS2_FIXPOSITION_DRIVER_NODE_HPP__
diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp
index 09349c1..3ebc93a 100644
--- a/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp
+++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/params.hpp
@@ -1,61 +1,52 @@
 /**
- *  @file
- *  @brief Parameters
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Parameters
  */
 
 #ifndef __FIXPOSITION_DRIVER_ROS2_PARAMS_HPP__
 #define __FIXPOSITION_DRIVER_ROS2_PARAMS_HPP__
 
-/* ROS */
-#include <fixposition_driver_ros2/ros2_msgs.hpp>
+/* LIBC/STL */
 
-/* FIXPOSITION */
+/* EXTERNAL */
 #include <fixposition_driver_lib/params.hpp>
+#include <fpsdk_ros2/ext/rclcpp.hpp>
+
+/* PACKAGE */
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 /**
- * @brief Load all parameters from ROS parameter server
+ * @brief Load sensor parameters from rosparam server
  *
- * @param[in] node
- * @param[in] ns namespace to load the parameters from
- * @param[out] params
- * @return true
- * @return false
- */
-bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node> node, const std::string& ns, FpOutputParams& params);
-
-/**
- * @brief Load all parameters from ROS parameter server
+ * @param[in]  nh      Node handle
+ * @param[in]  ns      Namespace to load the parameters from
+ * @param[out] params  The sensor parameters
  *
- * @param[in] node
- * @param[in] ns namespace to load the parameters from
- * @param[out] params
- * @return true
- * @return false
+ * @returns true on success, false otherwise
  */
-
-bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node> node, const std::string& ns, CustomerInputParams& params);
+bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node>& nh, const std::string& ns, DriverParams& params);
 
 /**
- * @brief Load all parameters from ROS parameter server
+ * @brief Load node parameters from rosparam server
  *
- * @param[in] node
- * @param[out] params
- * @return true
- * @return false
+ * @param[in]  nh      Node handle
+ * @param[in]  ns      Namespace to load the parameters from
+ * @param[out] params  The sensor parameters
+ *
+ * @returns true on success, false otherwise
  */
-bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node> node, FixpositionDriverParams& params);
+bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node>& nh, const std::string& ns, NodeParams& params);
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-
-#endif
+#endif  // __FIXPOSITION_DRIVER_ROS2_PARAMS_HPP__
diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp
index 32347f5..6e93464 100644
--- a/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp
+++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/ros2_msgs.hpp
@@ -1,72 +1,62 @@
-// Wrapper to include ROS2 stuff without quoting the same warning suppressions all over
-#ifndef __ROS2_DRIVER_ROS2_MSGS_HPP__
-#define __ROS2_DRIVER_ROS2_MSGS_HPP__
+// Wrapper to suppress warnings from ROS headers
+#ifndef __FIXPOSITION_DRIVER_ROS2_ROS2_MSGS_HPP__
+#define __FIXPOSITION_DRIVER_ROS2_ROS2_MSGS_HPP__
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #pragma GCC diagnostic ignored "-Wunused-parameter"
 #pragma GCC diagnostic ignored "-Wshadow"
 
-#include <rcl/rcl.h>
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp/logging.hpp>
-
-#include <std_msgs/msg/u_int8_multi_array.hpp>
+// Standard ROS messages
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
 
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
-
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
-
-#include <geometry_msgs/msg/vector3_stamped.hpp>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-
-#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2/LinearMath/Transform.h>
+#include <std_msgs/msg/u_int8_multi_array.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
-// Include generic
-#include <fixposition_driver_ros2/msg/speed.hpp>
-#include <fixposition_driver_ros2/msg/gnsssats.hpp>
-#include <fixposition_driver_ros2/msg/nmea.hpp>
-
-// Include RTCM
+// RTCM
 #include <rtcm_msgs/msg/message.hpp>
 
-// Include extras
-#include <fixposition_driver_ros2/msg/covwarn.hpp>
-
-// Include FP-A
-#include <fixposition_driver_ros2/msg/gnssant.hpp>
-#include <fixposition_driver_ros2/msg/gnsscorr.hpp>
-#include <fixposition_driver_ros2/msg/imubias.hpp>
-#include <fixposition_driver_ros2/msg/eoe.hpp>
-#include <fixposition_driver_ros2/msg/llh.hpp>
-#include <fixposition_driver_ros2/msg/odomenu.hpp>
-#include <fixposition_driver_ros2/msg/odometry.hpp>
-#include <fixposition_driver_ros2/msg/odomsh.hpp>
-#include <fixposition_driver_ros2/msg/odomstatus.hpp>
-#include <fixposition_driver_ros2/msg/text.hpp>
-#include <fixposition_driver_ros2/msg/tp.hpp>
-
-// Include NMEA
-#include <fixposition_driver_ros2/msg/gpgga.hpp>
-#include <fixposition_driver_ros2/msg/gpgll.hpp>
-#include <fixposition_driver_ros2/msg/gngsa.hpp>
-#include <fixposition_driver_ros2/msg/gpgst.hpp>
-#include <fixposition_driver_ros2/msg/gxgsv.hpp>
-#include <fixposition_driver_ros2/msg/gphdt.hpp>
-#include <fixposition_driver_ros2/msg/gprmc.hpp>
-#include <fixposition_driver_ros2/msg/gpvtg.hpp>
-#include <fixposition_driver_ros2/msg/gpzda.hpp>
-
-#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
-#include <tf2_eigen/tf2_eigen.hpp>
-#else
-// This header was deprecated as of ROS2 Humble, but is still required in order to support Foxy.
-#include <tf2_eigen/tf2_eigen.h>
-#endif
+// Fixposition ROS messages
+// - Generic
+#include <fixposition_driver_msgs/msg/nmea_epoch.hpp>
+#include <fixposition_driver_msgs/msg/parser_msg.hpp>
+#include <fixposition_driver_msgs/msg/speed.hpp>
+// - Extras
+#include <fixposition_driver_msgs/msg/cov_warn.hpp>
+// - FP-A
+#include <fixposition_driver_msgs/msg/fpa_consts.hpp>
+#include <fixposition_driver_msgs/msg/fpa_eoe.hpp>
+#include <fixposition_driver_msgs/msg/fpa_gnssant.hpp>
+#include <fixposition_driver_msgs/msg/fpa_gnsscorr.hpp>
+#include <fixposition_driver_msgs/msg/fpa_imubias.hpp>
+#include <fixposition_driver_msgs/msg/fpa_llh.hpp>
+#include <fixposition_driver_msgs/msg/fpa_odomenu.hpp>
+#include <fixposition_driver_msgs/msg/fpa_odometry.hpp>
+#include <fixposition_driver_msgs/msg/fpa_odomsh.hpp>
+#include <fixposition_driver_msgs/msg/fpa_odomstatus.hpp>
+#include <fixposition_driver_msgs/msg/fpa_text.hpp>
+#include <fixposition_driver_msgs/msg/fpa_tp.hpp>
+// - NMEA
+#include <fixposition_driver_msgs/msg/nmea_gga.hpp>
+#include <fixposition_driver_msgs/msg/nmea_gll.hpp>
+#include <fixposition_driver_msgs/msg/nmea_gsa.hpp>
+#include <fixposition_driver_msgs/msg/nmea_gst.hpp>
+#include <fixposition_driver_msgs/msg/nmea_gsv.hpp>
+#include <fixposition_driver_msgs/msg/nmea_hdt.hpp>
+#include <fixposition_driver_msgs/msg/nmea_rmc.hpp>
+#include <fixposition_driver_msgs/msg/nmea_vtg.hpp>
+#include <fixposition_driver_msgs/msg/nmea_zda.hpp>
+
+// Shortcut
+namespace fpmsgs = fixposition_driver_msgs::msg;
 
 #pragma GCC diagnostic pop
-#endif  // __ROS2_DRIVER_ROS2_MSGS_HPP__
+#endif  // __FIXPOSITION_DRIVER_ROS2_ROS2_MSGS_HPP__
diff --git a/fixposition_driver_ros2/launch/config.yaml b/fixposition_driver_ros2/launch/config.yaml
new file mode 100644
index 0000000..9bd6ffe
--- /dev/null
+++ b/fixposition_driver_ros2/launch/config.yaml
@@ -0,0 +1,61 @@
+/**:
+    ros__parameters:
+        # Configuration for the Vision-RTK 2 sensor
+        driver:
+            # Connection to the sensor one of:
+            # - Serial (<device>:<baudrate>)
+            # stream: serial:///dev/ttyUSB0:115200
+            # - TCP client (<ip_address>:<port>)
+            stream: tcpcli://172.22.1.44:21000
+            # Wait time in [s] to attempt reconnecting after connection lost
+            reconnect_delay: 5.0
+
+            # Messages that should be used by the driver. Note that the sensor must be configured accordingly for the correct
+            # port used in the "connection" above.
+            messages:
+                ##### Fusion output
+                # - Odometry and status
+                - "FP_A-ODOMETRY"          # configuration: FP_A-ODOMETRY
+                - "FP_A-ODOMENU"           # configuration: FP_A-ODOMENU
+                - "FP_A-ODOMSH"            # configuration: FP_A-ODOMSH
+                - "FP_A-ODOMSTATUS"        # configuration: FP_A-ODOMSTATUS
+                - "FP_A-IMUBIAS"           # configuration: FP_A-IMUBIAS
+                - "FP_A-EOE"               # configuration: FP_A-EOE_FUSION and FP_A-EOE_<epoch>
+                # - Transforms
+                - "FP_A-TF"                # configuration: FP_A-TF_{POIIMUH,VRTKCAM,POIVRTK,ECEFENU0,POIPOISH}
+                # - IMU data
+                - "FP_A-RAWIMU"            # configuration: FP_A-RAWIMU
+                - "FP_A-CORRIMU"           # configuration: FP_A-CORRIMU
+                # - LLH output
+                - "FP_A-LLH"               # configuration: FP_A-LLH
+                ##### Info messages
+                - "FP_A-TEXT"              # configuration: FP_A-TEXT_{ERROR,WARNING,INFO,DEBUG} (one or more)
+                ##### GNSS related data
+                - "FP_A-GNSSANT"           # configuration: FP_A-GNSSANT
+                - "FP_A-GNSSCORR"          # configuration: FP_A-GNSSCORR
+                - "FP_A-TP"                # configuration: FP_A-TP
+                ##### NMEA (see also nmea_epoch param below)
+                - "GGA"                    # configuration: NMEA-GN-GGA_<epoch> (or NMEA-GP-GGA_<epoch>)
+                - "GLL"                    # configuration: NMEA-GN-GLL_<epoch> (or NMEA-GP-GLL_<epoch>)
+                - "GSA"                    # configuration: NMEA-GN-GSA_<epoch>
+                - "GST"                    # configuration: NMEA-GN-GST_<epoch> (or NMEA-GP-GST_<epoch>)
+                - "GSV"                    # configuration: NMEA-GX-GSV_<epoch>
+                - "HDT"                    # configuration: NMEA-GN-HDT_<epoch> (or NMEA-GP-HDT_<epoch>)
+                - "RMC"                    # configuration: NMEA-GN-RMC_<epoch> (or NMEA-GP-RMC_<epoch>)
+                - "VTG"                    # configuration: NMEA-GN-VTG_<epoch> (or NMEA-GP-VTG_<epoch>)
+                - "ZDA"                    # configuration: NMEA-GN-ZDA_<epoch> (or NMEA-GP-ZDA_<epoch>)
+                ##### GNSS only position
+                - "NOV_B-BESTGNSSPOS"   # This can be one or both of the _GNSS1 and _GNSS2 variants
+
+            # Driver behaviour
+            nmea_epoch:  "GNSS"   # Choice for NMEA collection, must match NMEA message configuration type (<epoch> above)
+            raw_output:  false    # Enable raw messages output
+            cov_warning: false    # Enable covariance warnings
+            nav2_mode:   false    # Enable nav2 mode
+
+        # Node parameters
+        node:
+            output_ns:   "/fixposition"              # Namespace for output topics, leave empty to use node namespace
+            speed_topic: "/fixposition/speed"        # Wheelspeed input, empty to disable
+            corr_topic:  "/rtcm"                     # Correction data input, empty to disable
+            qos_type:    "sensor_short"              # "sensor_short", "sensor_long", "default_short", "default_long"
diff --git a/fixposition_driver_ros2/launch/dev.launch b/fixposition_driver_ros2/launch/dev.launch
new file mode 100644
index 0000000..01d4c93
--- /dev/null
+++ b/fixposition_driver_ros2/launch/dev.launch
@@ -0,0 +1,12 @@
+<!-- https://design.ros2.org/articles/roslaunch_xml.html -->
+<launch>
+    <arg name="node_name"   default="fixposition_driver_ros2" description="Node name"/> <!--  -->
+    <arg name="config"      default="config.yaml"             description="Configuration file to use"/>
+    <arg name="launcher"    default=""                        description="Launch node via this (node launch-prefix)"/>
+    <node name="$(var node_name)" pkg="fixposition_driver_ros2" exec="fixposition_driver_ros2_exec"
+        output="screen" respawn="true" respawn_delay="5" launch-prefix="$(var launcher)"
+        ros_args="--log-level $(var node_name):=debug">
+        <param from="$(find-pkg-share fixposition_driver_ros2)/launch/$(var config)"/>
+        <param name="some_numeric_param" value="100.2"/>
+    </node>
+</launch>
diff --git a/fixposition_driver_ros2/launch/node.launch b/fixposition_driver_ros2/launch/node.launch
new file mode 100644
index 0000000..d10ca73
--- /dev/null
+++ b/fixposition_driver_ros2/launch/node.launch
@@ -0,0 +1,12 @@
+<!-- https://design.ros2.org/articles/roslaunch_xml.html -->
+<launch>
+    <arg name="node_name"   default="fixposition_driver_ros2" description="Node name"/> <!--  -->
+    <arg name="config"      default="config.yaml"             description="Configuration file to use"/>
+    <arg name="launcher"    default=""                        description="Launch node via this (node launch-prefix)"/>
+    <node name="$(var node_name)" pkg="fixposition_driver_ros2" exec="fixposition_driver_ros2_exec"
+        output="screen" respawn="true" respawn_delay="5" launch-prefix="$(var launcher)"
+        ros_args="--log-level $(var node_name):=info">
+        <param from="$(find-pkg-share fixposition_driver_ros2)/launch/$(var config)"/>
+        <param name="some_numeric_param" value="100.2"/>
+    </node>
+</launch>
diff --git a/fixposition_driver_ros2/launch/serial.launch b/fixposition_driver_ros2/launch/serial.launch
deleted file mode 100644
index 38be8f3..0000000
--- a/fixposition_driver_ros2/launch/serial.launch
+++ /dev/null
@@ -1,11 +0,0 @@
-<launch>
-    <!--Change input_type and input_port according to the type of input source:
-               tcp - data over TCP
-               serial - data over a serial port
-        And according to the port used for reading, i.e.
-               21000 - TCP port
-               /dev/ttyUSB0 - serial port -->
-    <node name="fixposition_driver_ros2" pkg="fixposition_driver_ros2" exec="fixposition_driver_ros2_exec" output="screen" respawn="true" respawn_delay="5">
-    <param from="$(find-pkg-share fixposition_driver_ros2)/launch/serial.yaml" />
-    </node>
-</launch>
diff --git a/fixposition_driver_ros2/launch/serial.yaml b/fixposition_driver_ros2/launch/serial.yaml
deleted file mode 100644
index 3692448..0000000
--- a/fixposition_driver_ros2/launch/serial.yaml
+++ /dev/null
@@ -1,19 +0,0 @@
-fixposition_driver_ros2:
-  ros__parameters:
-    fp_output:
-      formats: [
-                "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", 
-                "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A
-                "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA
-               ]
-      type: "serial"
-      port: "/dev/ttyUSB0"
-      baudrate: 115200
-      rate: 200
-      reconnect_delay: 5.0 # wait time in [s] until retry connection
-      cov_warning: false
-      nav2_mode: false
-
-    customer_input:
-      speed_topic: "/fixposition/speed"
-      rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros2/launch/tcp.launch b/fixposition_driver_ros2/launch/tcp.launch
deleted file mode 100644
index 5d9cc3c..0000000
--- a/fixposition_driver_ros2/launch/tcp.launch
+++ /dev/null
@@ -1,12 +0,0 @@
-<launch>
-    <!--Change input_type and input_port according to the type of input source:
-               tcp - data over TCP
-               serial - data over a serial port
-        And according to the port used for reading, i.e.
-               21000 - TCP port
-               /dev/ttyUSB0 - serial port -->
-
-    <node name="fixposition_driver_ros2" pkg="fixposition_driver_ros2" exec="fixposition_driver_ros2_exec" output="screen" respawn="true" respawn_delay="5">
-    <param from="$(find-pkg-share fixposition_driver_ros2)/launch/tcp.yaml" />
-    </node>
-</launch>
diff --git a/fixposition_driver_ros2/launch/tcp.yaml b/fixposition_driver_ros2/launch/tcp.yaml
deleted file mode 100644
index 5955c72..0000000
--- a/fixposition_driver_ros2/launch/tcp.yaml
+++ /dev/null
@@ -1,20 +0,0 @@
-fixposition_driver_ros2:
-  ros__parameters:
-    fp_output:
-      formats: [
-                "ODOMETRY", "LLH", "ODOMENU", "ODOMSH", "ODOMSTATUS", "RAWIMU", "CORRIMU", 
-                "IMUBIAS", "GNSSANT", "GNSSCORR", "EOE", "TEXT", "TF", "TP", # FP_A
-                "GPGGA", "GPGLL", "GNGSA", "GPGST", "GPHDT", "GPRMC", "GPVTG", "GPZDA", "GXGSV" # NMEA
-               ]
-      type: "tcp"
-      port: "21000"
-      ip: "10.0.2.1" # change to VRTK2's IP address in the network
-      rate: 200
-      reconnect_delay: 5.0 # wait time in [s] until retry connection
-      qos_type: "sensor_short" # Supported types: "sensor_short", "sensor_long", "default_short", "default_long"
-      cov_warning: false
-      nav2_mode: false
-
-    customer_input:
-      speed_topic: "/fixposition/speed"
-      rtcm_topic: "/rtcm"
diff --git a/fixposition_driver_ros2/msg/Gnsssats.msg b/fixposition_driver_ros2/msg/Gnsssats.msg
deleted file mode 100644
index ecc5f7c..0000000
--- a/fixposition_driver_ros2/msg/Gnsssats.msg
+++ /dev/null
@@ -1,20 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition GNSS satellite signal statistics message.
-#
-#
-####################################################################################################
-
-# Format | Field         | Unit    | Description
-# -------|---------------|---------|------------------------------------|
-string     constellation # [Hex]   | Signal ID (see below).
-int16[]    sat_id        # [-]     | Satellite ID number.
-int16[]    azim          # [deg]   | Satellite azimuth from true North.
-int16[]    elev          # [deg]   | Satellite elevation.
-int16[]    cno_l1        # [db-Hz] | Satellite SNR (C/No) for L1-band.
-int16[]    cno_l2        # [db-Hz] | Satellite SNR (C/No) for L2-band.
diff --git a/fixposition_driver_ros2/msg/NMEA.msg b/fixposition_driver_ros2/msg/NMEA.msg
deleted file mode 100644
index c07852e..0000000
--- a/fixposition_driver_ros2/msg/NMEA.msg
+++ /dev/null
@@ -1,63 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA Message. Specified using the WGS 84 reference ellipsoid.
-#
-#
-####################################################################################################
-
-# Format        | Field       | Unit              | Description
-# --------------|-------------|-------------------|----------------------------------------------------------------------|
-std_msgs/Header   header      # [ns]              | Specifies the ROS time and Euclidian reference frame.
-string            time        # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-string            date        # [ddmmyy]          | UTC date (day, month and year).
-float64           latitude    # [ddmm.mmmmm(mm)]  | Latitude. Positive is north of equator; negative is south.
-float64           longitude   # [dddmm.mmmmm(mm)] | Longitude. Positive is east of prime meridian; negative is west.
-float64           altitude    # [m]               | Altitude. Positive is above the WGS 84 ellipsoid.
-int8              quality     # [-]               | Quality indicator (see below).
-int8              num_sv      # [-]               | Number of satellites. Strict NMEA: 00-12. High-precision NMEA: 00-99.
-int8[]            ids         # [-]               | ID numbers of satellites used in solution. See the NMEA 0183 version 4.11 standard document.
-float64           hdop_rec    # [0.10-99.99]      | Horizontal dilution of precision.
-float64           pdop        # [-]               | Position dillution of precision.
-float64           hdop        # [-]               | Horizontal dillution of precision.
-float64           vdop        # [-]               | Vertical dillution of precision.
-float64           rms_range   # [-]               | RMS value of the standard deviation of the range inputs to the navigation process.
-float64           std_major   # [m]               | Standard deviation of semi-major axis of error ellipse.
-float64           std_minor   # [m]               | Standard deviation of semi-minor axis of error ellipse.
-float64           angle_major # [deg]             | Angle of semi-major axis of error ellipse from true North.
-float64           std_lat     # [m]               | Standard deviation of latitude error.
-float64           std_lon     # [m]               | Standard deviation of longitude error.
-float64           std_alt     # [m]               | Standard deviation of altitude error.
-float64[9]        covariance  # [m2]              | Position covariance defined relative to a tangential plane (ENU frame).
-int8              cov_type    # [-]               | Method employed to estimate covariance (see below).
-float64           heading     # [deg]             | True heading.
-float64           speed       # [m/s]             | Speed over ground.
-float64           course      # [deg]             | Course over ground (w.r.t. True North).
-float64           diff_age    # [-]               | Approximate age of differential data (last GPS MSM message received).
-string            diff_sta    # [-]               | DGPS station ID (0000-1023).
-fixposition_driver_ros2/Gnsssats[] gnss_sats # [-]| GNSS satellite signal statistics.
-
-
-# Quality indicator table
-#
-# | ID | Signal         |
-# |----|----------------|
-# |  0 | Invalid        |
-# |  1 | Non-RTK fix    |
-# |  4 | RTK fixed      |
-# |  5 | RTK float      |
-# |  6 | Dead-reckoning |
-
-
-# Covariance type table
-#
-# | ID | Signal         |
-# |----|----------------|
-# |  0 | Unknown        |
-# |  1 | Approximated   |
-# |  2 | Diagonal known |
-# |  3 | Known          |
diff --git a/fixposition_driver_ros2/msg/Speed.msg b/fixposition_driver_ros2/msg/Speed.msg
deleted file mode 100644
index 9a5e295..0000000
--- a/fixposition_driver_ros2/msg/Speed.msg
+++ /dev/null
@@ -1,14 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Wheel speed input to Fixposition ROS Driver
-#
-#
-####################################################################################################
-
-# Velocity values for one or several WheelSensors
-fixposition_driver_ros2/WheelSensor[] sensors
diff --git a/fixposition_driver_ros2/msg/WheelSensor.msg b/fixposition_driver_ros2/msg/WheelSensor.msg
deleted file mode 100644
index c59889b..0000000
--- a/fixposition_driver_ros2/msg/WheelSensor.msg
+++ /dev/null
@@ -1,26 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition WheelSensor Message
-#
-#
-####################################################################################################
-# Standard metadata
-std_msgs/Header header
-
-# Location of the wheelspeed measurement (one of: RC, FR, FL, RR, RL)
-string location
-
-# Velocity values in [mm/s] as integer 32bit
-int32 vx
-int32 vy
-int32 vz
-
-# Velocity validity
-bool vx_valid
-bool vy_valid
-bool vz_valid
diff --git a/fixposition_driver_ros2/msg/extras/COVWARN.msg b/fixposition_driver_ros2/msg/extras/COVWARN.msg
deleted file mode 100644
index cfe6e43..0000000
--- a/fixposition_driver_ros2/msg/extras/COVWARN.msg
+++ /dev/null
@@ -1,15 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition Covariance Warning Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-geometry_msgs/Vector3 jump         # Position change
-geometry_msgs/Vector3 covariance   # Covariance
diff --git a/fixposition_driver_ros2/msg/fpa/EOE.msg b/fixposition_driver_ros2/msg/fpa/EOE.msg
deleted file mode 100644
index d1c541b..0000000
--- a/fixposition_driver_ros2/msg/fpa/EOE.msg
+++ /dev/null
@@ -1,14 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-EOE Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-string epoch   # Indicates which epoch ended (FUSION, GNSS, GNSS1 or GNSS2)
diff --git a/fixposition_driver_ros2/msg/fpa/GNSSANT.msg b/fixposition_driver_ros2/msg/fpa/GNSSANT.msg
deleted file mode 100644
index ef468f0..0000000
--- a/fixposition_driver_ros2/msg/fpa/GNSSANT.msg
+++ /dev/null
@@ -1,19 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-GNSSANT Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-string gnss1_state   # GNSS1 antenna state
-string gnss1_power   # GNSS1 antenna power
-int32  gnss1_age     # Time since gnss1_state or gnss1_power information last changed
-string gnss2_state   # GNSS2 antenna state
-string gnss2_power   # GNSS2 antenna power
-int32  gnss2_age     # Time since gnss2_state or gnss2_power information last changed
diff --git a/fixposition_driver_ros2/msg/fpa/GNSSCORR.msg b/fixposition_driver_ros2/msg/fpa/GNSSCORR.msg
deleted file mode 100644
index b9fc453..0000000
--- a/fixposition_driver_ros2/msg/fpa/GNSSCORR.msg
+++ /dev/null
@@ -1,28 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-GNSSCORR Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-int16 gnss1_fix          # GNSS1 fix status
-int16 gnss1_nsig_l1      # Number of L1 signals with correction data tracked by GNSS1
-int16 gnss1_nsig_l2      # Number of L2 signals with correction data tracked by GNSS1
-int16 gnss2_fix          # GNSS2 fix status
-int16 gnss2_nsig_l1      # Number of L1 signals with correction data tracked by GNSS2
-int16 gnss2_nsig_l2      # Number of L2 signals with correction data tracked by GNSS2
-
-float64 corr_latency       # Average correction data latency (10s window)
-float64 corr_update_rate   # Average correction update rate (10s window)
-float64 corr_data_rate     # Average correction data rate (10s window)
-float64 corr_msg_rate      # Average correction message rate (10s window)
-
-int16 sta_id                    # Correction station ID, range 0–4095
-geometry_msgs/Vector3 sta_llh   # Correction station LLH position (latitude, longitude, height)
-int32 sta_dist                  # Correction station baseline length
diff --git a/fixposition_driver_ros2/msg/fpa/IMUBIAS.msg b/fixposition_driver_ros2/msg/fpa/IMUBIAS.msg
deleted file mode 100644
index 10ae6fd..0000000
--- a/fixposition_driver_ros2/msg/fpa/IMUBIAS.msg
+++ /dev/null
@@ -1,68 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-IMUBIAS Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-int16 fusion_imu                     # Fusion measurement status: IMU (see below)
-int16 imu_status                     # IMU bias status (see below)
-int16 imu_noise                      # IMU variance status (see below)
-int16 imu_conv                       # IMU convergence status (see below)
-geometry_msgs/Vector3 bias_acc       # Accelerometer bias
-geometry_msgs/Vector3 bias_gyr       # Gyroscope bias
-geometry_msgs/Vector3 bias_cov_acc   # Accelerometer bias covariance
-geometry_msgs/Vector3 bias_cov_gyr   # Gyroscope bias covariance
-
-
-# Fusion measurement status (fusion_imu)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not used           |
-# |   1   | Used               |
-# |   2   | Degraded           |
-
-
-# IMU bias status (imu_status)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not converged      |
-# |   1   | Warmstarted        |
-# |   2   | Rough convergence  |
-# |   3   | Fine convergence   |
-
-
-# IMU variance (imu_noise)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Reserved           |
-# |   1   | Low noise          |
-# |   2   | Medium noise       |
-# |   3   | High noise         |
-# | 4...7 | Reserved           |
-
-
-# IMU accelerometer and gyroscope convergence (imu_conv)
-#
-# | Value | Description                      |
-# |-------|----------------------------------|
-# |  null | Info not available               |
-# |   0   | Awaiting Fusion                  |
-# |   1   | Awaiting IMU measurements        |
-# |   2   | Insufficient global measurements |
-# |   3   | Insufficient motion              |
-# |   4   | Converging                       |
-# | 5...6 | Reserved                         |
-# |   7   | Idle                             |
\ No newline at end of file
diff --git a/fixposition_driver_ros2/msg/fpa/LLH.msg b/fixposition_driver_ros2/msg/fpa/LLH.msg
deleted file mode 100644
index e5dd5e1..0000000
--- a/fixposition_driver_ros2/msg/fpa/LLH.msg
+++ /dev/null
@@ -1,16 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-LLH Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-string pose_frame                # frame of the pose values [pose, quaternion]
-geometry_msgs/Vector3 position   # LLH position (latitude, longitude, height)
-float64[9] covariance           # Position covariance in ENU
diff --git a/fixposition_driver_ros2/msg/fpa/ODOMENU.msg b/fixposition_driver_ros2/msg/fpa/ODOMENU.msg
deleted file mode 100644
index 3292f10..0000000
--- a/fixposition_driver_ros2/msg/fpa/ODOMENU.msg
+++ /dev/null
@@ -1,24 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMENU Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-string pose_frame                            # frame of the pose values [pose, quaternion]
-string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
-geometry_msgs/PoseWithCovariance pose        # position, orientation
-geometry_msgs/TwistWithCovariance velocity   # linear, angular
-geometry_msgs/Vector3 acceleration           # linear acceleration
-
-int16 fusion_status                          # field for the fusion status
-int16 imu_bias_status                        # field for the IMU bias status
-int16 gnss1_status                           # field for the gnss1 status
-int16 gnss2_status                           # field for the gnss2 status
-int16 wheelspeed_status                      # field for the wheelspeed status
diff --git a/fixposition_driver_ros2/msg/fpa/ODOMETRY.msg b/fixposition_driver_ros2/msg/fpa/ODOMETRY.msg
deleted file mode 100644
index 9b637ff..0000000
--- a/fixposition_driver_ros2/msg/fpa/ODOMETRY.msg
+++ /dev/null
@@ -1,25 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMETRY Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-string pose_frame                            # frame of the pose values [pose, quaternion]
-string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
-geometry_msgs/PoseWithCovariance pose        # position, orientation
-geometry_msgs/TwistWithCovariance velocity   # linear, angular
-geometry_msgs/Vector3 acceleration           # linear acceleration
-
-int16 fusion_status                          # field for the fusion status
-int16 imu_bias_status                        # field for the IMU bias status
-int16 gnss1_status                           # field for the gnss1 status
-int16 gnss2_status                           # field for the gnss2 status
-int16 wheelspeed_status                      # field for the wheelspeed status
-string version                               # Fixposition software version
diff --git a/fixposition_driver_ros2/msg/fpa/ODOMSH.msg b/fixposition_driver_ros2/msg/fpa/ODOMSH.msg
deleted file mode 100644
index 34fff6e..0000000
--- a/fixposition_driver_ros2/msg/fpa/ODOMSH.msg
+++ /dev/null
@@ -1,24 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMSH Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-string pose_frame                            # frame of the pose values [pose, quaternion]
-string kin_frame                             # frame of the kinematic values [linear/angular velocity, acceleration]
-geometry_msgs/PoseWithCovariance pose        # position, orientation
-geometry_msgs/TwistWithCovariance velocity   # linear, angular
-geometry_msgs/Vector3 acceleration           # linear acceleration
-
-int16 fusion_status                          # field for the fusion status
-int16 imu_bias_status                        # field for the IMU bias status
-int16 gnss1_status                           # field for the gnss1 status
-int16 gnss2_status                           # field for the gnss2 status
-int16 wheelspeed_status                      # field for the wheelspeed status
diff --git a/fixposition_driver_ros2/msg/fpa/ODOMSTATUS.msg b/fixposition_driver_ros2/msg/fpa/ODOMSTATUS.msg
deleted file mode 100644
index 7868a7b..0000000
--- a/fixposition_driver_ros2/msg/fpa/ODOMSTATUS.msg
+++ /dev/null
@@ -1,187 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-ODOMSTATUS Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-int16 init_status                            # Fusion init status (see below)
-int16 fusion_imu                             # Fusion measurement status: IMU (see below)
-int16 fusion_gnss1                           # Fusion measurement status: GNSS 1 (see below)
-int16 fusion_gnss2                           # Fusion measurement status: GNSS 2 (see below)
-int16 fusion_corr                            # Fusion measurement status: GNSS corrections (see below)
-int16 fusion_cam1                            # Fusion measurement status: camera (see below)
-int16 fusion_ws                              # Fusion measurement status: wheelspeed (see below)
-int16 fusion_markers                         # Fusion measurement status: markers (see below)
-int16 imu_status                             # IMU bias status (see below)
-int16 imu_noise                              # IMU variance status (see below)
-int16 imu_conv                               # IMU convergence status (see below)
-int16 gnss1_status                           # GNSS 1 status (see below)
-int16 gnss2_status                           # GNSS 2 status (see below)
-int16 baseline_status                        # Baseline status (see below)
-int16 corr_status                            # GNSS correction status (see below)
-int16 cam1_status                            # Camera 1 status (see below)
-int16 ws_status                              # Wheelspeed status (see below)
-int16 ws_conv                                # Wheelspeed convergence status (see below)
-int16 markers_status                         # Markers status (see below)
-int16 markers_conv                           # Markers convergence status (see below)
-
-
-# Fusion initialisation status (init_status)
-#
-# | Value | Description          |
-# |-------|----------------------|
-# |  null | Unknown              |
-# |   0   | Not initialized      |
-# |   1   | Locally initialised  |
-# |   2   | Globally initialised |
-
-
-# Fusion measurement status (fusion_imu, fusion_cam1, fusion_cam2, fusion_gnss1, fusion_gnss2, fusion_corr, fusion_ws, fusion_markers)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not used           |
-# |   1   | Used               |
-# |   2   | Degraded           |
-
-
-# IMU bias status (imu_status)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Not converged      |
-# |   1   | Warmstarted        |
-# |   2   | Rough convergence  |
-# |   3   | Fine convergence   |
-
-
-# IMU variance (imu_noise)
-#
-# | Value | Description        |
-# |-------|--------------------|
-# |  null | Info not available |
-# |   0   | Reserved           |
-# |   1   | Low noise          |
-# |   2   | Medium noise       |
-# |   3   | High noise         |
-# | 4...7 | Reserved           |
-
-
-# IMU accelerometer and gyroscope convergence (imu_conv)
-#
-# | Value | Description                      |
-# |-------|----------------------------------|
-# |  null | Info not available               |
-# |   0   | Awaiting Fusion                  |
-# |   1   | Awaiting IMU measurements        |
-# |   2   | Insufficient global measurements |
-# |   3   | Insufficient motion              |
-# |   4   | Converging                       |
-# | 5...6 | Reserved                         |
-# |   7   | Idle                             |
-
-
-# GNSS fix status (gnss1_status, gnss2_status)
-#
-# | Value | Description                    |
-# |-------|--------------------------------|
-# |  null | Info not available             |
-# |   0   | No fix                         |
-# |   1   | Single-point positioning (SPP) |
-# |   2   | RTK moving baseline            |
-# | 3...4 | Reserved                       |
-# |   5   | RTK float                      |
-# | 6...7 | Reserved                       |
-# |   8   | RTK fixed                      |
-
-
-# GNSS correction status (corr_status)
-#
-# | Value | Description                                                                                                    |
-# |-------|----------------------------------------------------------------------------------------------------------------|
-# |  null | Info not available                                                                                             |
-# |   0   | Waiting fusion                                                                                                 |
-# |   1   | No GNSS available                                                                                              |
-# |   2   | No corrections used                                                                                            |
-# |   3   | Limited corrections used: station data & at least one of the constellations among the valid rover measurements |
-# |   4   | Corrections are too old                                                                                        |
-# |   5   | Sufficient corrections used: station data and corrections for ALL bands among the valid rover measurements     |
-
-
-# Baseline status (baseline_status)
-#
-# | Value | Description            |
-# |-------|------------------------|
-# |  null | Info not available     |
-# |   0   | Waiting fusion         |
-# |   1   | Not available / No fix |
-# |   2   | Failing                |
-# |   3   | Passing                |
-
-
-# Camera status (cam1_status)
-#
-# | Value | Description                                      |
-# |-------|--------------------------------------------------|
-# |  null | Info not available                               |
-# |   0   | Camera not available                             |
-# |   1   | Camera available, but not usable (e.g. too dark) |
-# | 2...4 | Reserved                                         |
-# |   5   | Camera working and available                     |
-
-
-# Wheelspeed status (ws_status)
-#
-# | Value | Description                                                |
-# |-------|------------------------------------------------------------|
-# |  null | Info not available                                         |
-# |   0   | No wheelspeed enabled                                      |
-# |   1   | Missing wheelspeed measurements                            |
-# |   2   | At least one wheelspeed enabled, no wheelspeed converged   |
-# |   3   | At least one wheelspeed enabled and at least one converged |
-# |   4   | At least one wheelspeed enabled and all converged          |
-
-
-# Wheelspeed convergence status (ws_conv)
-#
-# | Value | Description                       |
-# |-------|-----------------------------------|
-# |  null | Info not available                |
-# |   0   | Awaiting Fusion                   |
-# |   1   | Missing wheelspeed measurements   |
-# |   2   | Insufficient global measurements  |
-# |   3   | Insufficient motion               |
-# |   4   | Insufficient imu bias convergence |
-# |   5   | Converging                        |
-# |   6   | Idle                              |
-
-
-# Markers status (markers_status)
-#
-# | Value | Description                 |
-# |-------|-----------------------------|
-# |  null | Info not available          |
-# |   0   | No markers available        |
-# |   1   | Markers available           |
-# |   2   | Markers available, and used |
-
-
-# Markers convergence status (markers_conv)
-#
-# | Value | Description                      |
-# |-------|----------------------------------|
-# |  null | Info not available               |
-# |   0   | Awaiting Fusion                  |
-# |   1   | Waiting marker measurements      |
-# |   2   | Insufficient global measurements |
-# |   3   | Converging                       |
-# |   4   | Idle                             |
diff --git a/fixposition_driver_ros2/msg/fpa/TEXT.msg b/fixposition_driver_ros2/msg/fpa/TEXT.msg
deleted file mode 100644
index 96e0cce..0000000
--- a/fixposition_driver_ros2/msg/fpa/TEXT.msg
+++ /dev/null
@@ -1,15 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-TEXT Message
-#
-#
-####################################################################################################
-
-std_msgs/Header header
-string level   # One of: INFO, WARNING, ERROR, DEBUG
-string text    # Text
diff --git a/fixposition_driver_ros2/msg/fpa/TP.msg b/fixposition_driver_ros2/msg/fpa/TP.msg
deleted file mode 100644
index 47dde39..0000000
--- a/fixposition_driver_ros2/msg/fpa/TP.msg
+++ /dev/null
@@ -1,53 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition FP_A-TP Message
-#
-#
-####################################################################################################
-
-string  tp_name                              # Timepulse name (source)
-string  timebase                             # Time base (see below), or null if not available
-string  timeref                              # Time reference (see below), or null if not available
-int64   tp_tow_sec                           # Timepulse time seconds of week, integer second part (0–604799), or null
-float64 tp_tow_psec                          # Timepulse time seconds of week, sub-second part (0.000000000000–0.999999999999), or null
-int64   gps_leaps                            # GPS leapseconds, or null if unknown
-
-
-# Values for timebase
-#
-# | Value | Description               |
-# |-------|---------------------------|
-# |  null | No timepulse alignment    |
-# |  GNSS | Timepulse aligned to GNSS |
-# |   UTC | Timepulse aligned to UTC  |
-
-
-# Values for timeref if timebase is GNSS
-#
-# | Value | Description                     |
-# |-------|---------------------------------|
-# |   GPS | Timepulse aligned to GPS        |
-# |   GAL | Timepulse aligned to Galileo    |
-# |   BDS | Timepulse aligned to BeiDou     |
-# |   GLO | Timepulse aligned to GLONASS    |
-# | OTHER | Timepulse aligned to other GNSS |
-
-
-# Values for timeref if timebase is UTC
-#
-# | Value | Description                                                                |
-# |-------|----------------------------------------------------------------------------|
-# |  NONE | Timepulse aligned to no UTC (no precise UTC parameters known yet)          |
-# |   CRL | Timepulse aligned to Communications Research Laboratory (CRL), Japan       |
-# |  NIST | Timepulse aligned to National Institute of Standards and Technology (NIST) |
-# |  USNO | Timepulse aligned to U.S. Naval Observatory (USNO)                         |
-# |  BIPM | Timepulse aligned to International Bureau of Weights and Measures (BIPM)   |
-# |    EU | Timepulse aligned to European laboratories                                 |
-# |    SU | Timepulse aligned to Former Soviet Union (SU)                              |
-# |  NTSC | Timepulse aligned to National Time Service Center (NTSC), China            |
-# | OTHER | Timepulse aligned to other/unknown UTC                                     |
diff --git a/fixposition_driver_ros2/msg/nmea/GNGSA.msg b/fixposition_driver_ros2/msg/nmea/GNGSA.msg
deleted file mode 100644
index 80481b7..0000000
--- a/fixposition_driver_ros2/msg/nmea/GNGSA.msg
+++ /dev/null
@@ -1,21 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GN-GSA Message
-#
-#
-####################################################################################################
-
-# Format | Field    | Unit | Description
-# -------|----------|------|----------------------------------------------------------------------------------------------|
-char       mode_op  #  [-] | Operation mode: always A (automatic, allowed to automatically switch 2D/3D).
-int8       mode_nav #  [-] | Navigation mode: 1 (fix not available), 2 (2D) or 3 (3D).
-int8[]     ids      #  [-] | ID numbers of satellites used in solution. See the NMEA 0183 version 4.11 standard document.
-float64    pdop     #  [-] | Position dillution of precision.
-float64    hdop     #  [-] | Horizontal dillution of precision.
-float64    vdop     #  [-] | Vertical dillution of precision.
-int8       gnss_id  #  [-] | GNSS system ID: 1 (GPS, SBAS), 2 (GLONASS), 3 (Galileo), 4 (BeiDou), 5 (QZSS).
diff --git a/fixposition_driver_ros2/msg/nmea/GPGGA.msg b/fixposition_driver_ros2/msg/nmea/GPGGA.msg
deleted file mode 100644
index 4201d18..0000000
--- a/fixposition_driver_ros2/msg/nmea/GPGGA.msg
+++ /dev/null
@@ -1,37 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-GGA Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit              | Description
-# -------|-----------|-------------------|----------------------------------------------------------------------|
-string     time      # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-float64    latitude  # [ddmm.mmmmm(mm)]  | Latitude.
-char       lat_ns    # [-]               | Latitude north (N) or south (S) indicator.
-float64    longitude # [dddmm.mmmmm(mm)] | Longitude.
-char       lon_ew    # [-]               | Longitude east (E) or west (W) indicator.
-int8       quality   # [-]               | Quality indicator (see below).
-int8       num_sv    # [-]               | Number of satellites. Strict NMEA: 00-12. High-precision NMEA: 00-99.
-float64    hdop      # [0.10-99.99]      | Horizontal dilution of precision.
-float64    alt       # [m]               | Altitude (above ellipsoid).
-char       alt_unit  # [-]               | Altitude unit, always M (metres).
-float64    diff_age  # [-]               | Approximate age of differential data (last GPS MSM message received).
-string     diff_sta  # [-]               | DGPS station ID (0000-1023).
-string     sentence  # [-]               | ASCII string to be used by NTRIP clients.
-
-# Quality indicator table
-#
-# | ID | Signal         |
-# |----|----------------|
-# |  0 | Invalid        |
-# |  1 | Non-RTK fix    |
-# |  4 | RTK fixed      |
-# |  5 | RTK float      |
-# |  6 | Dead-reckoning |
diff --git a/fixposition_driver_ros2/msg/nmea/GPGLL.msg b/fixposition_driver_ros2/msg/nmea/GPGLL.msg
deleted file mode 100644
index 7b067e2..0000000
--- a/fixposition_driver_ros2/msg/nmea/GPGLL.msg
+++ /dev/null
@@ -1,32 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-GLL Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit              | Description
-# -------|-----------|-------------------|--------------------------------------------------------|
-float64    latitude  # [ddmm.mmmmm(mm)]  | Latitude.
-char       lat_ns    # [-]               | Latitude north (N) or south (S) indicator.
-float64    longitude # [dddmm.mmmmm(mm)] | Longitude.
-char       lon_ew    # [-]               | Longitude east (E) or west (W) indicator.
-string     time      # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-char       status    # [-]               | Data validity status, A (data valid) or V (not valid).
-char       mode      # [-]               | Positioning system mode indicator (see below).
-
-# Mode table
-#
-# | ID | Signal                |
-# |----|-----------------------|
-# |  N | Data not valid        |
-# |  E | Dead-reckoning        |
-# |  D | Differential          |
-# |  A | Autonomous            |
-# |  M | Manual input not used |
-# |  S | Simulator not used    |
diff --git a/fixposition_driver_ros2/msg/nmea/GPGST.msg b/fixposition_driver_ros2/msg/nmea/GPGST.msg
deleted file mode 100644
index 56d26a4..0000000
--- a/fixposition_driver_ros2/msg/nmea/GPGST.msg
+++ /dev/null
@@ -1,22 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-GST Message
-#
-#
-####################################################################################################
-
-# Format | Field       | Unit            | Description
-# -------|-------------|-----------------|------------------------------------------------------------------------------------|
-string     time        # [hhmmss.ss(ss)] | UTC time (hours, minutes and seconds).
-float64    rms_range   # [-]             | RMS value of the standard deviation of the range inputs to the navigation process.
-float64    std_major   # [m]             | Standard deviation of semi-major axis of error ellipse.
-float64    std_minor   # [m]             | Standard deviation of semi-minor axis of error ellipse.
-float64    angle_major # [deg]           | Angle of semi-major axis of error ellipse from true North.
-float64    std_lat     # [m]             | Standard deviation of latitude error.
-float64    std_lon     # [m]             | Standard deviation of longitude error.
-float64    std_alt     # [m]             | Standard deviation of altitude error.
diff --git a/fixposition_driver_ros2/msg/nmea/GPHDT.msg b/fixposition_driver_ros2/msg/nmea/GPHDT.msg
deleted file mode 100644
index 5e4b001..0000000
--- a/fixposition_driver_ros2/msg/nmea/GPHDT.msg
+++ /dev/null
@@ -1,16 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-HDT Message
-#
-#
-####################################################################################################
-
-# Format | Field    | Unit  | Description
-# -------|----------|-------|---------------|
-float64    heading  # [deg] | True heading.
-char       true_ind # [-]   | Always T.
\ No newline at end of file
diff --git a/fixposition_driver_ros2/msg/nmea/GPRMC.msg b/fixposition_driver_ros2/msg/nmea/GPRMC.msg
deleted file mode 100644
index 060e4fd..0000000
--- a/fixposition_driver_ros2/msg/nmea/GPRMC.msg
+++ /dev/null
@@ -1,35 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-RMC Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit              | Description
-# -------|-----------|-------------------|--------------------------------------------------------|
-string     time      # [hhmmss.ss(ss)]   | UTC time (hours, minutes and seconds).
-char       status    # [-]               | Data validity status, A (data valid) or V (not valid).
-float64    latitude  # [ddmm.mmmmm(mm)]  | Latitude.
-char       lat_ns    # [-]               | Latitude north (N) or south (S) indicator.
-float64    longitude # [dddmm.mmmmm(mm)] | Longitude.
-char       lon_ew    # [-]               | Longitude east (E) or west (W) indicator.
-float64    speed     # [knots]           | Speed over ground.
-float64    course    # [deg]             | Course over ground (w.r.t. True North).
-string     date      # [ddmmyy]          | UTC date (day, month and year).
-char       mode      # [-]               | Positioning system mode indicator (see below).
-
-# Mode table
-#
-# | ID | Signal                |
-# |----|-----------------------|
-# |  N | Data not valid        |
-# |  E | Dead-reckoning        |
-# |  D | Differential          |
-# |  A | Autonomous            |
-# |  M | Manual input not used |
-# |  S | Simulator not used    |
diff --git a/fixposition_driver_ros2/msg/nmea/GPVTG.msg b/fixposition_driver_ros2/msg/nmea/GPVTG.msg
deleted file mode 100644
index 81fd5db..0000000
--- a/fixposition_driver_ros2/msg/nmea/GPVTG.msg
+++ /dev/null
@@ -1,34 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-VTG Message
-#
-#
-####################################################################################################
-
-# Format | Field      | Unit    | Description
-# -------|------------|---------|--------------------------------------------------------|
-float64    cog_true   # [deg]   | Course over ground wrt. True North.
-char       cog_ref_t  # [-]     | COG reference, always T (true).
-float64    cog_mag    # [-]     | Course over ground w.r.t. Magnetic North, always null.
-char       cog_ref_m  # [-]     | COG reference, always M (magnetic).
-float64    sog_knot   # [knots] | Speed over ground in knots.
-char       sog_unit_n # [-]     | SOG reference, always N (knots).
-float64    sog_kph    # [km/h]  | Speed over ground in km/h.
-char       sog_unit_k # [-]     | SOG reference, always K (km/h).
-char       mode       # [-]     | Positioning system mode indicator (see below).
-
-# Mode table
-#
-# | ID | Signal                |
-# |----|-----------------------|
-# |  N | Data not valid        |
-# |  E | Dead-reckoning        |
-# |  D | Differential          |
-# |  A | Autonomous            |
-# |  M | Manual input not used |
-# |  S | Simulator not used    |
diff --git a/fixposition_driver_ros2/msg/nmea/GPZDA.msg b/fixposition_driver_ros2/msg/nmea/GPZDA.msg
deleted file mode 100644
index 747bb0c..0000000
--- a/fixposition_driver_ros2/msg/nmea/GPZDA.msg
+++ /dev/null
@@ -1,19 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GP-ZDA Message
-#
-#
-####################################################################################################
-
-# Format        | Field     | Unit            | Description
-# --------------|-----------|-----------------|-------------------------------------------------------|
-std_msgs/Header   header    # [ns]            | Specifies the ROS time and Euclidian reference frame.
-string            time      # [hhmmss.ss(ss)] | UTC0 time (hours, minutes and seconds).
-string            date      # [dd/mm/yyyy]    | Date (UTC0).
-int16             local_hr  # [00+-13]        | Local zone hours, always 00 (= UTC).
-int16             local_min # [00-59]         | Local zone minutes, always 00 (= UTC).
diff --git a/fixposition_driver_ros2/msg/nmea/GXGSV.msg b/fixposition_driver_ros2/msg/nmea/GXGSV.msg
deleted file mode 100644
index be4fc4c..0000000
--- a/fixposition_driver_ros2/msg/nmea/GXGSV.msg
+++ /dev/null
@@ -1,36 +0,0 @@
-####################################################################################################
-#
-#    Copyright (c) 2023
-#    Fixposition AG
-#
-####################################################################################################
-#
-# Fixposition NMEA-GX-GSV Message
-#
-#
-####################################################################################################
-
-# Format | Field     | Unit    | Description
-# -------|-----------|---------|-------------------------------------|
-int16      sentences # [-]     | Total number of sentences (1 to 9).
-int16      sent_num  # [-]     | Sentence number (1 to 9).
-int32      num_sats  # [-]     | Total number of satellites in view.
-int16[]    sat_id    # [-]     | Satellite ID number.               
-int16[]    elev      # [deg]   | Satellite elevation. 
-int16[]    azim      # [deg]   | Satellite azimuth from true North.
-int16[]    cno       # [db-Hz] | Satellite SNR (C/No).
-string     signal_id # [Hex]   | Signal ID (see below).
-
-# Signal ID table
-#
-# | ID | Signal                        |
-# |----|-------------------------------|
-# |  0 | No signal (any talker ID)     |
-# |  1 | GPS/SBAS L1C/A (talker ID GP) |
-# |  6 | GPS L2C-L (talker ID GP)      |
-# |  7 | Galileo L1-BC (talker ID GA)  |
-# |  2 | Galileo E5b (talker ID GA)    |
-# |  1 | BeiDou B1I (talker ID GB)     |
-# |  B | BeiDou B2I (talker ID GB)     |
-# |  1 | GLONASS G1 C/A (talker ID GL) |
-# |  3 | GLONASS G2 C/A (talker ID GL) |
\ No newline at end of file
diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml
index bf40bcf..45e974f 100644
--- a/fixposition_driver_ros2/package.xml
+++ b/fixposition_driver_ros2/package.xml
@@ -1,11 +1,9 @@
 <?xml version="1.0"?>
 <package format="3">
     <name>fixposition_driver_ros2</name>
-    <version>5.0.0</version>
-    <description>Fixposition ROS Driver</description>
-
-    <maintainer email="info@fixposition.com">Fixposition AG</maintainer>
-
+    <version>8.0.0</version>
+    <description>Fixposition ROS2 driver</description>
+    <maintainer email="support@fixposition.com">Fixposition Support</maintainer>
     <license>MIT</license>
 
     <buildtool_depend>ament_cmake</buildtool_depend>
@@ -16,6 +14,7 @@
     <build_depend>builtin_interfaces</build_depend>
 
     <depend>fixposition_driver_lib</depend>
+    <depend>fixposition_driver_msgs</depend>
     <depend>geometry_msgs</depend>
     <depend>nav_msgs</depend>
     <depend>sensor_msgs</depend>
@@ -25,6 +24,8 @@
     <depend>tf2_ros</depend>
     <depend>tf2_geometry_msgs</depend>
     <depend>rtcm_msgs</depend>
+    <depend>fpsdk_common</depend>
+    <depend>fpsdk_ros2</depend>
 
     <exec_depend>rclcpp</exec_depend>
     <exec_depend>rosidl_default_runtime</exec_depend>
diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp
index 28048fb..3056652 100644
--- a/fixposition_driver_ros2/src/data_to_ros2.cpp
+++ b/fixposition_driver_ros2/src/data_to_ros2.cpp
@@ -1,813 +1,757 @@
 /**
- *  @file
- *  @brief Convert Data classes to ROS2 msgs
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Convert data to ROS2 msgs
  */
 
+/* LIBC/STL */
+
+/* EXTERNAL */
+#include <fixposition_driver_msgs/data_to_ros.hpp>
+#include <fpsdk_common/math.hpp>
+#include <fpsdk_common/time.hpp>
+#include <fpsdk_common/trafo.hpp>
+#include <fpsdk_ros2/utils.hpp>
+
 /* PACKAGE */
-#include <fixposition_driver_ros2/data_to_ros2.hpp>
+#include "fixposition_driver_ros2/data_to_ros2.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-void FpToRosMsg(const fixposition::OdometryData& data, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        nav_msgs::msg::Odometry msg;
-        
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
-        }
-        msg.header.frame_id = data.frame_id;
-        msg.child_frame_id = data.child_frame_id;
+using namespace fpsdk;
+using namespace fpsdk::common;
+using namespace fpsdk::common::parser;
 
-        PoseWithCovDataToMsg(data.pose, msg.pose);
-        TwistWithCovDataToMsg(data.twist, msg.twist);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
-        pub->publish(msg);
-    }
+static void PoseWithCovDataToMsg(const PoseWithCovData& data, geometry_msgs::msg::PoseWithCovariance& msg) {
+    msg.pose.position = tf2::toMsg(data.position);
+    msg.pose.orientation = tf2::toMsg(data.orientation);
+    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
+    cov_map = data.cov;
 }
 
-void FpToRosMsg(const ImuData& data, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        sensor_msgs::msg::Imu msg;
-        
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
-        }
-        msg.header.frame_id = data.frame_id;
+static void TwistWithCovDataToMsg(const TwistWithCovData& data, geometry_msgs::msg::TwistWithCovariance& msg) {
+    tf2::toMsg(data.linear, msg.twist.linear);
+    tf2::toMsg(data.angular, msg.twist.angular);
+    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
+    cov_map = data.cov;
+}
 
-        tf2::toMsg(data.linear_acceleration, msg.linear_acceleration);
-        tf2::toMsg(data.angular_velocity, msg.angular_velocity);
+void TfDataToTransformStamped(const TfData& data, geometry_msgs::msg::TransformStamped& msg) {
+    msg.header.stamp = ros2::utils::ConvTime(data.stamp);
+    msg.header.frame_id = data.frame_id;
+    msg.child_frame_id = data.child_frame_id;
+    msg.transform.rotation = tf2::toMsg(data.rotation);
+    tf2::toMsg(data.translation, msg.transform.translation);
+}
 
-        // Publish message
-        pub->publish(msg);
-    }
+void OdometryDataToTransformStamped(const OdometryData& data, geometry_msgs::msg::TransformStamped& msg) {
+    msg.header.stamp = ros2::utils::ConvTime(data.stamp);
+    msg.header.frame_id = data.frame_id;
+    msg.child_frame_id = data.child_frame_id;
+    msg.transform.rotation = tf2::toMsg(data.pose.orientation);
+    tf2::toMsg(data.pose.position, msg.transform.translation);
 }
 
+// ---------------------------------------------------------------------------------------------------------------------
 
-void FpToRosMsg(const FP_IMUBIAS& data, rclcpp::Publisher<fixposition_driver_ros2::msg::IMUBIAS>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::IMUBIAS msg;
+template <typename SomeFpaOdoPayload, typename SomeOdoMsg>
+static void FpaOdomToRos(const SomeFpaOdoPayload& payload, SomeOdoMsg& msg) {
+    msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
 
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
-        }
-        
-        msg.header.frame_id = data.frame_id;
-        msg.fusion_imu = data.fusion_imu;
-        msg.imu_status = data.imu_status;
-        msg.imu_noise = data.imu_noise;
-        msg.imu_conv = data.imu_conv;
-        tf2::toMsg(data.bias_acc, msg.bias_acc);
-        tf2::toMsg(data.bias_gyr, msg.bias_gyr);
-        tf2::toMsg(data.bias_cov_acc, msg.bias_cov_acc);
-        tf2::toMsg(data.bias_cov_gyr, msg.bias_cov_gyr);
+    msg.fusion_status = FpaFusionStatusLegacyToMsg(msg, payload.fusion_status);
+    msg.imu_bias_status = FpaImuStatusLegacyToMsg(msg, payload.imu_bias_status);
+    msg.gnss1_status = FpaGnssFixToMsg(msg, payload.gnss1_fix);
+    msg.gnss2_status = FpaGnssFixToMsg(msg, payload.gnss2_fix);
+    msg.wheelspeed_status = FpaWsStatusLegacyToMsg(msg, payload.wheelspeed_status);
 
-        // Publish message
+    FpaFloat3ToVector3(payload.acc, msg.acceleration);
+
+    PoseWithCovData pose;
+    pose.SetFromFpaOdomPayload(payload);
+    PoseWithCovDataToMsg(pose, msg.pose);
+
+    TwistWithCovData velocity;
+    velocity.SetFromFpaOdomPayload(payload);
+    TwistWithCovDataToMsg(velocity, msg.velocity);
+}
+
+void PublishFpaOdometry(const fpa::FpaOdometryPayload& payload,
+                        rclcpp::Publisher<fpmsgs::FpaOdometry>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaOdometry msg;
+        msg.header.frame_id = ODOMETRY_FRAME_ID;
+        msg.pose_frame = ODOMETRY_CHILD_FRAME_ID;
+        msg.kin_frame = ODOMETRY_CHILD_FRAME_ID;
+        FpaOdomToRos(payload, msg);
+        msg.version = payload.version;
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_GNSSANT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSANT>::SharedPtr pub) {
+void PublishFpaOdomenu(const fpa::FpaOdomenuPayload& payload, rclcpp::Publisher<fpmsgs::FpaOdomenu>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GNSSANT msg;
-        
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
-        }
-
-        msg.gnss1_state = data.gnss1_state;
-        msg.gnss1_power = data.gnss1_power;
-        msg.gnss1_age = data.gnss1_age;
-        msg.gnss2_state = data.gnss2_state;
-        msg.gnss2_power = data.gnss2_power;
-        msg.gnss2_age = data.gnss2_age;
-        
-        // Publish message
+        fpmsgs::FpaOdomenu msg;
+        msg.header.frame_id = ODOMENU_FRAME_ID;
+        msg.pose_frame = ODOMENU_CHILD_FRAME_ID;
+        msg.kin_frame = ODOMENU_CHILD_FRAME_ID;
+        FpaOdomToRos(payload, msg);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_GNSSCORR& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNSSCORR>::SharedPtr pub) {
+void PublishFpaOdomsh(const fpa::FpaOdomshPayload& payload, rclcpp::Publisher<fpmsgs::FpaOdomsh>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GNSSCORR msg;
-        
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
-        }
-
-        msg.gnss1_fix = data.gnss1_fix;
-        msg.gnss1_nsig_l1 = data.gnss1_nsig_l1;
-        msg.gnss1_nsig_l2 = data.gnss1_nsig_l2;
-        msg.gnss2_fix = data.gnss2_fix;
-        msg.gnss2_nsig_l1 = data.gnss2_nsig_l1;
-        msg.gnss2_nsig_l2 = data.gnss2_nsig_l2;
-
-        msg.corr_latency = data.corr_latency;
-        msg.corr_update_rate = data.corr_update_rate;
-        msg.corr_data_rate = data.corr_data_rate;
-        msg.corr_msg_rate = data.corr_msg_rate;
+        fpmsgs::FpaOdomsh msg;
+        msg.header.frame_id = ODOMSH_FRAME_ID;
+        msg.pose_frame = ODOMSH_CHILD_FRAME_ID;
+        msg.kin_frame = ODOMSH_CHILD_FRAME_ID;
+        FpaOdomToRos(payload, msg);
+        pub->publish(msg);
+    }
+}
 
-        msg.sta_id = data.sta_id;
-        tf2::toMsg(data.sta_llh, msg.sta_llh);
-        msg.sta_dist = data.sta_dist;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaOdometryDataImu(const fpa::FpaOdometryPayload& payload,
+                               rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        sensor_msgs::msg::Imu msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = ODOMETRY_FRAME_ID;
+        FpaFloat3ToVector3(payload.acc, msg.linear_acceleration);
+        FpaFloat3ToVector3(payload.rot, msg.angular_velocity);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_LLH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::LLH>::SharedPtr pub) {
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishFpaOdometryDataNavSatFix(const fpa::FpaOdometryPayload& payload,
+                                     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::LLH msg;
+        sensor_msgs::msg::NavSatFix msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = ODOMSH_CHILD_FRAME_ID;
 
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
+        // Populate LLH position
+        PoseWithCovData pose;
+        pose.SetFromFpaOdomPayload(payload);
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
+            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
+
+        if (pose.position.isZero()) {
+            msg.position_covariance_type = msg.COVARIANCE_TYPE_UNKNOWN;
+            cov_map = Eigen::Matrix3d::Zero();  // FIXME: necessary?
         } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
+            const Eigen::Vector3d llh_pos = trafo::TfWgs84LlhEcef(pose.position);
+            msg.latitude = math::RadToDeg(llh_pos(0));
+            msg.longitude = math::RadToDeg(llh_pos(1));
+            msg.altitude = llh_pos(2);
+
+            // Populate LLH covariance
+            const Eigen::Matrix3d p_cov_e = pose.cov.topLeftCorner(3, 3);
+            const Eigen::Matrix3d C_l_e = trafo::RotEnuEcef(pose.position);
+            const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose();
+            cov_map = p_cov_l;
+            msg.position_covariance_type = msg.COVARIANCE_TYPE_KNOWN;
         }
 
-        tf2::toMsg(data.llh, msg.position);
-        Eigen::Map<Eigen::Matrix3d> cov_map = Eigen::Map<Eigen::Matrix3d>(msg.covariance.data());
-        cov_map = data.cov;
+        // Populate LLH status
+        const fpa::FpaGnssFix fix = (payload.gnss1_fix > payload.gnss2_fix ? payload.gnss1_fix : payload.gnss2_fix);
+        msg.status.status = FpaGnssFixToNavSatStatusStatus(msg.status, fix);
+        if (msg.status.status != msg.status.STATUS_NO_FIX) {
+            msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS |
+                                  msg.status.SERVICE_GALILEO);
+        }
 
         // Publish message
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMENU& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMENU>::SharedPtr pub) {
+// ---------------------------------------------------------------------------------------------------------------------
+
+void PublishFpaOdomenuVector3Stamped(const fpa::FpaOdomenuPayload& payload,
+                                     rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::ODOMENU msg;
+        geometry_msgs::msg::Vector3Stamped msg;
 
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.odom.stamp);
-        }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        msg.pose_frame = data.odom.child_frame_id;
-        msg.kin_frame = data.odom.child_frame_id;
-
-        PoseWithCovDataToMsg(data.odom.pose, msg.pose);
-        TwistWithCovDataToMsg(data.odom.twist, msg.velocity);
-        tf2::toMsg(data.acceleration, msg.acceleration);
-        msg.fusion_status = data.fusion_status;
-        msg.imu_bias_status = data.imu_bias_status;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.wheelspeed_status = data.wheelspeed_status;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = ENU_FRAME_ID;
+
+        const Eigen::Quaterniond quat = {payload.orientation.values[0], payload.orientation.values[1],
+                                         payload.orientation.values[2], payload.orientation.values[3]};
+
+        // Euler angle wrt. ENU frame in the order of yaw pitch roll
+        const Eigen::Vector3d enu_euler = trafo::RotToEul(quat.toRotationMatrix());
+        // const Eigen::Vector3d enu_euler = trafo::QuatToEul(quat); // FIXME couldn't we just use this?
+        msg.vector.set__x(enu_euler.x());
+        msg.vector.set__y(enu_euler.y());
+        msg.vector.set__z(enu_euler.z());
 
         // Publish message
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMETRY& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMETRY>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::ODOMETRY msg;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.odom.stamp);
-        }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        msg.pose_frame = data.odom.child_frame_id;
-        msg.kin_frame = data.odom.child_frame_id;
-
-        PoseWithCovDataToMsg(data.odom.pose, msg.pose);
-        TwistWithCovDataToMsg(data.odom.twist, msg.velocity);
-        tf2::toMsg(data.acceleration, msg.acceleration);
-        msg.fusion_status = data.fusion_status;
-        msg.imu_bias_status = data.imu_bias_status;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.wheelspeed_status = data.wheelspeed_status;
-        msg.version = data.version;
-        
-        // Publish message
+void PublishFpaOdomstatus(const fpa::FpaOdomstatusPayload& payload,
+                          rclcpp::Publisher<fpmsgs::FpaOdomstatus>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaOdomstatus msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+
+        // clang-format off
+        msg.init_status     = FpaInitStatusToMsg(msg, payload.init_status);
+        msg.fusion_imu      = FpaMeasStatusToMsg(msg, payload.fusion_imu);
+        msg.fusion_gnss1    = FpaMeasStatusToMsg(msg, payload.fusion_gnss1);
+        msg.fusion_gnss2    = FpaMeasStatusToMsg(msg, payload.fusion_gnss2);
+        msg.fusion_corr     = FpaMeasStatusToMsg(msg, payload.fusion_corr);
+        msg.fusion_cam1     = FpaMeasStatusToMsg(msg, payload.fusion_cam1);
+        msg.fusion_ws       = FpaMeasStatusToMsg(msg, payload.fusion_ws);
+        msg.fusion_markers  = FpaMeasStatusToMsg(msg, payload.fusion_markers);
+        msg.imu_status      = FpaImuStatusToMsg(msg, payload.imu_status);
+        msg.imu_noise       = FpaImuNoiseToMsg(msg, payload.imu_noise);
+        msg.imu_conv        = FpaImuConvToMsg(msg, payload.imu_conv);
+        msg.gnss1_status    = FpaGnssStatusToMsg(msg, payload.gnss1_status);
+        msg.gnss2_status    = FpaGnssStatusToMsg(msg, payload.gnss2_status);
+        msg.baseline_status = FpaBaselineStatusToMsg(msg, payload.baseline_status);
+        msg.corr_status     = FpaCorrStatusToMsg(msg, payload.corr_status);
+        msg.cam1_status     = FpaCamStatusToMsg(msg, payload.cam1_status);
+        msg.ws_status       = FpaWsStatusToMsg(msg, payload.ws_status);
+        msg.ws_conv         = FpaWsConvToMsg(msg, payload.ws_conv);
+        msg.markers_status  = FpaMarkersStatusToMsg(msg, payload.markers_status);
+        msg.markers_conv    = FpaMarkersConvToMsg(msg, payload.markers_conv);
+        // clang-format on
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMSH& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSH>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::ODOMSH msg;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.odom.stamp);
+void PublishFpaLlh(const fpa::FpaLlhPayload& payload, rclcpp::Publisher<fpmsgs::FpaLlh>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaLlh msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        FpaFloat3ToVector3(payload.llh, msg.position);
+        if (payload.cov_enu.valid) {
+            Eigen::Map<Eigen::Matrix3d> cov_map = Eigen::Map<Eigen::Matrix3d>(msg.covariance.data());
+            cov_map = BuildCovMat3D(payload.cov_enu.values[0], payload.cov_enu.values[1], payload.cov_enu.values[2],
+                                    payload.cov_enu.values[3], payload.cov_enu.values[4], payload.cov_enu.values[5]);
         }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        msg.pose_frame = data.odom.child_frame_id;
-        msg.kin_frame = data.odom.child_frame_id;
-
-        PoseWithCovDataToMsg(data.odom.pose, msg.pose);
-        TwistWithCovDataToMsg(data.odom.twist, msg.velocity);
-        tf2::toMsg(data.acceleration, msg.acceleration);
-        msg.fusion_status = data.fusion_status;
-        msg.imu_bias_status = data.imu_bias_status;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.wheelspeed_status = data.wheelspeed_status;
-
-        // Publish message
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_ODOMSTATUS& data, rclcpp::Publisher<fixposition_driver_ros2::msg::ODOMSTATUS>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::ODOMSTATUS msg;
-
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
-        }
-
-        msg.init_status = data.init_status;
-        msg.fusion_imu = data.fusion_imu;
-        msg.fusion_gnss1 = data.fusion_gnss1;
-        msg.fusion_gnss2 = data.fusion_gnss2;
-        msg.fusion_corr = data.fusion_corr;
-        msg.fusion_cam1 = data.fusion_cam1;
-        msg.fusion_ws = data.fusion_ws;
-        msg.fusion_markers = data.fusion_markers;
-        msg.imu_status = data.imu_status;
-        msg.imu_noise = data.imu_noise;
-        msg.imu_conv = data.imu_conv;
-        msg.gnss1_status = data.gnss1_status;
-        msg.gnss2_status = data.gnss2_status;
-        msg.baseline_status = data.baseline_status;
-        msg.corr_status = data.corr_status;
-        msg.cam1_status = data.cam1_status;
-        msg.ws_status = data.ws_status;
-        msg.ws_conv = data.ws_conv;
-        msg.markers_status = data.markers_status;
-        msg.markers_conv = data.markers_conv;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaEoe(const fpa::FpaEoePayload& payload, rclcpp::Publisher<fpmsgs::FpaEoe>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaEoe msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.epoch = FpaEpochToMsg(msg, payload.epoch);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_TEXT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TEXT>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::TEXT msg;
-
-        // Populate message
-        msg.level = data.level;
-        msg.text = data.text;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaImubias(const fpa::FpaImubiasPayload& payload, rclcpp::Publisher<fpmsgs::FpaImubias>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaImubias msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.header.frame_id = IMU_FRAME_ID;
+        msg.fusion_imu = FpaMeasStatusToMsg(msg, payload.fusion_imu);
+        msg.imu_status = FpaImuStatusToMsg(msg, payload.imu_status);
+        msg.imu_noise = FpaImuNoiseToMsg(msg, payload.imu_noise);
+        msg.imu_conv = FpaImuConvToMsg(msg, payload.imu_conv);
+        FpaFloat3ToVector3(payload.bias_acc, msg.bias_acc);
+        FpaFloat3ToVector3(payload.bias_gyr, msg.bias_gyr);
+        FpaFloat3ToVector3(payload.bias_cov_acc, msg.bias_cov_acc);
+        FpaFloat3ToVector3(payload.bias_cov_gyr, msg.bias_cov_gyr);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_TP& data, rclcpp::Publisher<fixposition_driver_ros2::msg::TP>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::TP msg;
-
-        // Populate message
-        msg.tp_name = data.tp_name;
-        msg.timebase = data.timebase;
-        msg.timeref = data.timeref;
-        msg.tp_tow_sec = data.tp_tow_sec;
-        msg.tp_tow_psec = data.tp_tow_psec;
-        msg.gps_leaps = data.gps_leaps;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaGnssant(const fpa::FpaGnssantPayload& payload, rclcpp::Publisher<fpmsgs::FpaGnssant>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaGnssant msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.gnss1_state = FpaAntStateToMsg(msg, payload.gnss1_state);
+        msg.gnss1_power = FpaAntPowerToMsg(msg, payload.gnss1_power);
+        msg.gnss1_age = (payload.gnss1_age.valid ? payload.gnss1_age.value : -1);
+        msg.gnss2_state = FpaAntStateToMsg(msg, payload.gnss2_state);
+        msg.gnss2_power = FpaAntPowerToMsg(msg, payload.gnss2_power);
+        msg.gnss2_age = (payload.gnss2_age.valid ? payload.gnss2_age.value : -1);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const FP_EOE& data, rclcpp::Publisher<fixposition_driver_ros2::msg::EOE>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::EOE msg;
-
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
-        }
-        msg.epoch = data.epoch;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaGnsscorr(const fpa::FpaGnsscorrPayload& payload,
+                        rclcpp::Publisher<fpmsgs::FpaGnsscorr>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaGnsscorr msg;
+        msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+        msg.gnss1_fix = FpaGnssFixToMsg(msg, payload.gnss1_fix);
+        msg.gnss1_nsig_l1 = (payload.gnss1_nsig_l1.valid ? payload.gnss1_nsig_l1.value : -1);
+        msg.gnss1_nsig_l2 = (payload.gnss1_nsig_l2.valid ? payload.gnss1_nsig_l2.value : -1);
+        msg.gnss2_fix = FpaGnssFixToMsg(msg, payload.gnss2_fix);
+        msg.gnss2_nsig_l1 = (payload.gnss2_nsig_l1.valid ? payload.gnss2_nsig_l1.value : -1);
+        msg.gnss2_nsig_l2 = (payload.gnss2_nsig_l2.valid ? payload.gnss2_nsig_l2.value : -1);
+        msg.corr_latency = (payload.corr_latency.valid ? payload.corr_latency.value : 0.0f);
+        msg.corr_update_rate = (payload.corr_update_rate.valid ? payload.corr_update_rate.value : 0.0f);
+        msg.corr_data_rate = (payload.corr_data_rate.valid ? payload.corr_data_rate.value : 0.0f);
+        msg.corr_msg_rate = (payload.corr_msg_rate.valid ? payload.corr_msg_rate.value : 0.0f);
+        msg.sta_id = (payload.sta_id.valid ? payload.sta_id.value : -1);
+        FpaFloat3ToVector3(payload.sta_llh, msg.sta_llh);
+        msg.sta_dist = (payload.sta_dist.valid ? payload.sta_dist.value : -1);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_GGA& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPGGA>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GPGGA msg;
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.latitude = data.llh(0);
-        msg.lat_ns = data.lat_ns;
-        msg.longitude = data.llh(1);
-        msg.lon_ew = data.lon_ew;
-        msg.quality = data.quality;
-        msg.num_sv = data.num_sv;
-        msg.hdop = data.hdop;
-        msg.alt = data.llh(2);
-        msg.alt_unit = data.alt_unit;
-        msg.diff_age = data.diff_age;
-        msg.diff_sta = data.diff_sta;
-        msg.sentence = data.sentence;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaTp(const fpa::FpaTpPayload& payload, rclcpp::Publisher<fpmsgs::FpaTp>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaTp msg;
+        msg.tp_name = payload.tp_name;
+        msg.timebase = FpaTimebaseToMsg(msg, payload.timebase);
+        msg.timeref = FpaTimerefToMsg(msg, payload.timeref);
+        msg.tp_week = payload.tp_week.value;
+        msg.tp_tow_sec = payload.tp_tow_sec.value;
+        msg.tp_tow_psec = payload.tp_tow_psec.value;
+        msg.gps_leaps = payload.gps_leaps.value;
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_GLL& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPGLL>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GPGLL msg;
-
-        // Populate message
-        msg.latitude = data.latlon(0);
-        msg.lat_ns = data.lat_ns;
-        msg.longitude = data.latlon(1);
-        msg.lon_ew = data.lon_ew;
-        msg.time = data.time_str;
-        msg.status = data.status;
-        msg.mode = data.mode;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishFpaText(const fpa::FpaTextPayload& payload, rclcpp::Publisher<fpmsgs::FpaText>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::FpaText msg;
+        msg.level = FpaTextLevelToMsg(msg, payload.level);
+        msg.text = payload.text;
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const GN_GSA& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GNGSA>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GNGSA msg;
-
-        // Populate message
-        msg.mode_op = data.mode_op;
-        msg.mode_nav = data.mode_nav;
-        
-        for (unsigned int i = 0; i < data.ids.size(); i++) {
-           msg.ids.push_back(data.ids.at(i));
-        }
-        
-        msg.pdop = data.pdop;
-        msg.hdop = data.hdop;
-        msg.vdop = data.vdop;
-        msg.gnss_id = data.gnss_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
-        pub->publish(msg);
+template <typename SomeFpaImuPayload>
+static void FpaImuPayloadToRos(const SomeFpaImuPayload& payload, sensor_msgs::msg::Imu msg) {
+    msg.header.stamp = ros2::utils::ConvTime(FpaGpsTimeToTime(payload.gps_time));
+    msg.header.frame_id = IMU_FRAME_ID;
+    if (payload.acc.valid) {
+        msg.linear_acceleration.x = payload.acc.values[0];
+        msg.linear_acceleration.y = payload.acc.values[1];
+        msg.linear_acceleration.z = payload.acc.values[2];
+    }
+    if (payload.rot.valid) {
+        msg.angular_velocity.x = payload.rot.values[0];
+        msg.angular_velocity.y = payload.rot.values[1];
+        msg.angular_velocity.z = payload.rot.values[2];
     }
 }
 
-void FpToRosMsg(const GP_GST& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPGST>::SharedPtr pub) {
+void PublishFpaRawimu(const fpa::FpaRawimuPayload& payload, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GPGST msg;
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.rms_range = data.rms_range;
-        msg.std_major = data.std_major;
-        msg.std_minor = data.std_minor;
-        msg.angle_major = data.angle_major;
-        msg.std_lat = data.std_lat;
-        msg.std_lon = data.std_lon;
-        msg.std_alt = data.std_alt;
-        
-        // Publish message
+        sensor_msgs::msg::Imu msg;
+        FpaImuPayloadToRos(payload, msg);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const GX_GSV& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GXGSV>::SharedPtr pub) {
+void PublishFpaCorrimu(const fpa::FpaCorrimuPayload& payload,
+                       rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GXGSV msg;
-
-        // Populate message
-        msg.sentences = data.sentences;
-        msg.sent_num = data.sent_num;
-        msg.num_sats = data.num_sats;
-        
-        for (unsigned int i = 0; i < data.sat_id.size(); i++) {
-            msg.sat_id.push_back(data.sat_id.at(i));
-            msg.elev.push_back(data.elev.at(i));
-            msg.azim.push_back(data.azim.at(i));
-            msg.cno.push_back(data.cno.at(i));
-        }
-
-        msg.signal_id = data.signal_id;
-
-        // Publish message
+        sensor_msgs::msg::Imu msg;
+        FpaImuPayloadToRos(payload, msg);
         pub->publish(msg);
     }
 }
 
+// ---------------------------------------------------------------------------------------------------------------------
 
-void FpToRosMsg(const GP_HDT& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPHDT>::SharedPtr pub) {
+bool PublishNovbBestgnsspos(const novb::NovbHeader* header, const novb::NovbBestgnsspos* payload,
+                            rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub1,
+                            rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr& pub2) {
+    if ((header == NULL) || (payload == NULL) || !header->IsLongHeader() ||
+        !((header->Source() == novb::NovbMsgTypeSource::PRIMARY) ||
+          (header->Source() == novb::NovbMsgTypeSource::SECONDARY))) {
+        return false;
+    }
+    auto pub = (header->Source() == novb::NovbMsgTypeSource::PRIMARY ? pub1 : pub2);
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GPHDT msg;
+        sensor_msgs::msg::NavSatFix msg;
 
-        // Populate message
-        msg.heading = data.heading;
-        msg.true_ind = data.true_ind;
+        time::Time stamp;
+        if (stamp.SetWnoTow({header->long_header.gps_week, (double)header->long_header.gps_milliseconds * 1e-3,
+                             time::WnoTow::Sys::GPS})) {
+            msg.header.stamp = ros2::utils::ConvTime(stamp);
+        }
+
+        msg.header.frame_id = (header->Source() == novb::NovbMsgTypeSource::PRIMARY ? GNSS1_FRAME_ID : GNSS2_FRAME_ID);
+        msg.status.status =
+            NovbPosOrVelTypeToNavSatStatusStatus(msg.status, static_cast<novb::NovbPosOrVelType>(payload->pos_type));
+        if (msg.status.status != msg.status.STATUS_NO_FIX) {
+            msg.status.service = (msg.status.SERVICE_GPS | msg.status.SERVICE_GLONASS | msg.status.SERVICE_COMPASS |
+                                  msg.status.SERVICE_GALILEO);
+        }
+
+        msg.latitude = payload->lat;
+        msg.longitude = payload->lon;
+        msg.altitude = payload->hgt;
+
+        msg.position_covariance_type = msg.COVARIANCE_TYPE_DIAGONAL_KNOWN;
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
+            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
+
+        Eigen::Array3d cov_diag(payload->lat_stdev, payload->lon_stdev, payload->hgt_stdev);
+        cov_map = (cov_diag * cov_diag).matrix().asDiagonal();
 
-        // Publish message
         pub->publish(msg);
     }
+    return true;
 }
 
-void FpToRosMsg(const GP_RMC& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPRMC>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GPRMC msg;
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.status = data.status;
-        msg.latitude = data.latlon(0);
-        msg.lat_ns = data.lat_ns;
-        msg.longitude = data.latlon(1);
-        msg.lon_ew = data.lon_ew;
-        msg.speed = data.speed;
-        msg.course = data.course;
-        msg.date = data.date_str;
-        msg.mode = data.mode;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishNmeaGga(const fpsdk::common::parser::nmea::NmeaGgaPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGga>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaGga msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.latitude = (payload.llh.latlon_valid ? payload.llh.lat : NAN);
+        msg.longitude = (payload.llh.latlon_valid ? payload.llh.lon : NAN);
+        msg.height = (payload.llh.height_valid ? payload.llh.height : NAN);
+        msg.quality = NmeaQualityGgaToMsg(msg, payload.quality);
+        msg.num_sv = (payload.num_sv.valid ? payload.num_sv.value : -1);
+        msg.hdop = (payload.hdop.valid ? payload.hdop.value : NAN);
+        msg.diff_age = (payload.diff_age.valid ? payload.diff_age.value : NAN);
+        msg.diff_sta = payload.diff_sta.value;
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_VTG& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPVTG>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GPVTG msg;
-
-        // Populate message
-        msg.cog_true = data.cog_true;
-        msg.cog_ref_t = data.cog_ref_t;
-        msg.cog_mag = data.cog_mag;
-        msg.cog_ref_m = data.cog_ref_m;
-        msg.sog_knot = data.sog_knot;
-        msg.sog_unit_n = data.sog_unit_n;
-        msg.sog_kph = data.sog_kph;
-        msg.sog_unit_k = data.sog_unit_k;
-        msg.mode = data.mode;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishNmeaGll(const fpsdk::common::parser::nmea::NmeaGllPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGll>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaGll msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.latitude = (payload.ll.latlon_valid ? payload.ll.lat : NAN);
+        msg.longitude = (payload.ll.latlon_valid ? payload.ll.lon : NAN);
+        msg.status = NmeaStatusGllRmcToMsg(msg, payload.status);
+        msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode);
         pub->publish(msg);
     }
 }
 
-void FpToRosMsg(const GP_ZDA& data, rclcpp::Publisher<fixposition_driver_ros2::msg::GPZDA>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::GPZDA msg;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // ROS Header
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
+void PublishNmeaGsa(const fpsdk::common::parser::nmea::NmeaGsaPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGsa>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaGsa msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.system = NmeaSystemIdToMsg(msg, payload.system);
+        msg.opmode = NmeaOpModeGsaToMsg(msg, payload.opmode);
+        msg.navmode = NmeaNavModeGsaToMsg(msg, payload.navmode);
+        for (auto& sat : payload.sats) {
+            if (sat.valid) {
+                msg.sats.push_back(sat.svid);
+            }
         }
-        msg.header.frame_id = "FP_POI";
-
-        // Populate message
-        msg.time = data.time_str;
-        msg.date = data.date_str;
-        msg.local_hr = data.local_hr;
-        msg.local_min = data.local_min;
-
-        // Publish message
+        msg.pdop = (payload.pdop.valid ? payload.pdop.value : NAN);
+        msg.hdop = (payload.hdop.valid ? payload.hdop.value : NAN);
+        msg.vdop = (payload.vdop.valid ? payload.vdop.value : NAN);
         pub->publish(msg);
     }
 }
 
-void TfDataToMsg(const TfData& data, geometry_msgs::msg::TransformStamped& msg) {
-    msg.header.frame_id = data.frame_id;
-    msg.child_frame_id = data.child_frame_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-    if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-        msg.header.stamp = rclcpp::Clock().now();
-    } else {
-        msg.header.stamp = GpsTimeToMsgTime(data.stamp);
+void PublishNmeaGst(const fpsdk::common::parser::nmea::NmeaGstPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGst>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaGst msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.rms_range = (payload.rms_range.valid ? payload.rms_range.value : NAN);
+        msg.std_major = (payload.std_major.valid ? payload.std_major.value : NAN);
+        msg.std_minor = (payload.std_minor.valid ? payload.std_minor.value : NAN);
+        msg.angle_major = (payload.angle_major.valid ? payload.angle_major.value : NAN);
+        msg.std_lat = (payload.std_lat.valid ? payload.std_lat.value : NAN);
+        msg.std_lon = (payload.std_lon.valid ? payload.std_lon.value : NAN);
+        msg.std_alt = (payload.std_alt.valid ? payload.std_alt.value : NAN);
+        pub->publish(msg);
     }
-
-    msg.transform.rotation = tf2::toMsg(data.rotation);
-    tf2::toMsg(data.translation, msg.transform.translation);
 }
+// ---------------------------------------------------------------------------------------------------------------------
 
-void NavSatFixDataToMsg(const NavSatFixData& data, sensor_msgs::msg::NavSatFix& msg) {
-    if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-        msg.header.stamp = rclcpp::Clock().now();
-    } else {
-        msg.header.stamp = GpsTimeToMsgTime(data.stamp);
+void PublishNmeaGsv(const fpsdk::common::parser::nmea::NmeaGsvPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaGsv>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaGsv msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.system = NmeaSystemIdToMsg(msg, payload.system);
+        msg.signal = NmeaSignalIdToMsg(msg, payload.signal);
+        msg.num_msgs = payload.num_msgs.value;
+        msg.msg_num = payload.msg_num.value;
+        for (auto& azel : payload.azels) {
+            if (azel.valid) {
+                msg.azel_sat.push_back(azel.svid);
+                msg.az.push_back(azel.az);
+                msg.el.push_back(azel.el);
+            }
+        }
+        for (auto& cno : payload.cnos) {
+            if (cno.valid) {
+                msg.cno_sat.push_back(cno.svid);
+                msg.cno.push_back(cno.cno);
+            }
+        }
+        pub->publish(msg);
     }
-    msg.header.frame_id = data.frame_id;
-    msg.status.status = data.status.status;
-    msg.status.service = data.status.service;
-    msg.latitude = data.latitude;
-    msg.longitude = data.longitude;
-    msg.altitude = data.altitude;
-
-    Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
-        Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
-    cov_map = data.cov;
-
-    msg.position_covariance_type = data.position_covariance_type;
 }
 
-void PoseWithCovDataToMsg(const PoseWithCovData& data, geometry_msgs::msg::PoseWithCovariance& msg) {
-    msg.pose.position = tf2::toMsg(data.position);
-    msg.pose.orientation = tf2::toMsg(data.orientation);
+// ---------------------------------------------------------------------------------------------------------------------
 
-    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
-    cov_map = data.cov;
+void PublishNmeaHdt(const fpsdk::common::parser::nmea::NmeaHdtPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaHdt>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaHdt msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.heading = (payload.heading.valid ? payload.heading.value : NAN);
+        pub->publish(msg);
+    }
 }
 
-void TwistWithCovDataToMsg(const fixposition::TwistWithCovData& data, geometry_msgs::msg::TwistWithCovariance& msg) {
-    tf2::toMsg(data.linear, msg.twist.linear);
-    tf2::toMsg(data.angular, msg.twist.angular);
-
-    Eigen::Map<Eigen::Matrix<double, 6, 6>> cov_map = Eigen::Map<Eigen::Matrix<double, 6, 6>>(msg.covariance.data());
-    cov_map = data.cov;
-}
+// ---------------------------------------------------------------------------------------------------------------------
 
-void OdometryDataToTf(const FP_ODOMETRY& data, std::shared_ptr<tf2_ros::TransformBroadcaster> pub) {
-    if (data.fusion_status > 0) {
-        // Ensure Fusion orientation is a valid quaternion
-        if (data.odom.pose.orientation.vec().isZero() && (data.odom.pose.orientation.w() == 0)) {
-            return;
+void PublishNmeaRmc(const fpsdk::common::parser::nmea::NmeaRmcPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaRmc>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaRmc msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.date.valid) {
+            msg.date_valid = true;
+            msg.date_y = payload.date.years;
+            msg.date_m = payload.date.months;
+            msg.date_d = payload.date.days;
         }
-
-        // Populate message
-        geometry_msgs::msg::TransformStamped msg;
-        OdomToTf(data.odom, msg);
-
-        // Publish message
-        pub->sendTransform(msg);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
+        }
+        msg.status = NmeaStatusGllRmcToMsg(msg, payload.status);
+        msg.mode = NmeaModeRmcGnsToMsg(msg, payload.mode);
+        msg.navstatus = NmeaNavStatusRmcToMsg(msg, payload.navstatus);
+        msg.latitude = (payload.llh.latlon_valid ? payload.llh.lat : NAN);
+        msg.longitude = (payload.llh.latlon_valid ? payload.llh.lon : NAN);
+        msg.height = (payload.llh.height_valid ? payload.llh.height : NAN);
+        msg.speed = (payload.speed.valid ? payload.speed.value : NAN);
+        msg.course = (payload.course.valid ? payload.course.value : NAN);
+        pub->publish(msg);
     }
 }
 
-void OdomToTf(const OdometryData& data, geometry_msgs::msg::TransformStamped& tf) {
-    // Populate message
-    tf.header.frame_id = data.frame_id;
-    tf.child_frame_id = data.child_frame_id;
+// ---------------------------------------------------------------------------------------------------------------------
 
-    if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-        tf.header.stamp = rclcpp::Clock().now();
-    } else {
-        tf.header.stamp = GpsTimeToMsgTime(data.stamp);
+void PublishNmeaVtg(const fpsdk::common::parser::nmea::NmeaVtgPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaVtg>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaVtg msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        msg.cogt = (payload.cogt.valid ? payload.cogt.value : NAN);
+        msg.cogm = (payload.cogm.valid ? payload.cogm.value : NAN);
+        msg.sogn = (payload.sogn.valid ? payload.sogn.value : NAN);
+        msg.sogk = (payload.sogk.valid ? payload.sogk.value : NAN);
+        msg.mode = NmeaModeGllVtgToMsg(msg, payload.mode);
+        pub->publish(msg);
     }
-
-    tf.transform.rotation = tf2::toMsg(data.pose.orientation);
-    tf2::toMsg(data.pose.position, tf.transform.translation);
 }
 
-void PublishNav2Tf(const std::map<std::string, std::shared_ptr<geometry_msgs::msg::TransformStamped>>& tf_map, std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_br_, std::shared_ptr<tf2_ros::TransformBroadcaster> br_) {
-    if (tf_map.at("ECEFENU0") && tf_map.at("POIPOISH") && tf_map.at("ECEFPOISH") && tf_map.at("ENU0POI")) {
-        // Publish FP_ECEF -> map
-        tf_map.at("ECEFENU0")->child_frame_id = "map";
-        static_br_->sendTransform(*tf_map.at("ECEFENU0"));
-
-        // Compute FP_ENU0 -> FP_POISH
-        // Extract translation and rotation from ECEFENU0
-        geometry_msgs::msg::Vector3 trans_ecef_enu0 = tf_map.at("ECEFENU0")->transform.translation;
-        geometry_msgs::msg::Quaternion rot_ecef_enu0 = tf_map.at("ECEFENU0")->transform.rotation;
-        Eigen::Vector3d t_ecef_enu0_;
-        t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z;
-        Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z);
-
-        // Extract translation and rotation from ECEFPOISH
-        geometry_msgs::msg::Vector3 trans_ecef_poish = tf_map.at("ECEFPOISH")->transform.translation;
-        geometry_msgs::msg::Quaternion rot_ecef_poish = tf_map.at("ECEFPOISH")->transform.rotation;
-        Eigen::Vector3d t_ecef_poish;
-        t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z;
-        Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z);
-
-        // Compute the ENU transformation
-        const Eigen::Vector3d t_enu0_poish = fixposition::TfEnuEcef(t_ecef_poish, fixposition::TfWgs84LlhEcef(t_ecef_enu0_));
-        const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish;
-
-        // Create tf2::Transform tf_ENU0POISH
-        tf2::Transform tf_ENU0POISH;
-        tf_ENU0POISH.setOrigin(tf2::Vector3(t_enu0_poish.x(), t_enu0_poish.y(), t_enu0_poish.z()));
-        tf2::Quaternion tf_q_enu0_poish(q_enu0_poish.x(), q_enu0_poish.y(), q_enu0_poish.z(), q_enu0_poish.w());
-        tf_ENU0POISH.setRotation(tf_q_enu0_poish);
-
-        // Publish map -> odom
-        // Multiply the transforms
-        tf2::Transform tf_ENU0POI;
-        tf2::fromMsg(tf_map.at("ENU0POI")->transform, tf_ENU0POI);
-        tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse();
-
-        // Create a new TransformStamped message
-        geometry_msgs::msg::TransformStamped tf_map_odom;
-        tf_map_odom.header.stamp = rclcpp::Clock().now();
-        tf_map_odom.header.frame_id = "map";
-        tf_map_odom.child_frame_id = "odom";
-        tf_map_odom.transform = tf2::toMsg(tf_combined);
-        br_->sendTransform(tf_map_odom);
-
-        // Publish odom -> base_link
-        geometry_msgs::msg::TransformStamped tf_odom_base;
-        tf_odom_base.header.stamp = rclcpp::Clock().now();
-        tf_odom_base.header.frame_id = "odom";
-        tf_odom_base.child_frame_id = "base_link";
-        tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Send the transform
-        br_->sendTransform(tf_odom_base);
-    }
-}
-
-void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr pub) {
+void PublishNmeaZda(const fpsdk::common::parser::nmea::NmeaZdaPayload& payload,
+                    rclcpp::Publisher<fpmsgs::NmeaZda>::SharedPtr& pub) {
     if (pub->get_subscription_count() > 0) {
-        // Create message
-        sensor_msgs::msg::NavSatFix msg;
-    
-        // Populate message header
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.odom.stamp);
-        }
-        msg.header.frame_id = data.odom.child_frame_id;
-        
-        // Populate LLH position
-        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
-            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.position_covariance.data());
-        
-        if (data.odom.pose.position.isZero()) {
-            msg.latitude  = 0;
-            msg.longitude = 0;
-            msg.altitude  = 0;
-            msg.position_covariance_type = 0;
-            cov_map = Eigen::Matrix3d::Zero();
-        } else {
-            const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position);
-            msg.latitude  = RadToDeg(llh_pos(0));
-            msg.longitude = RadToDeg(llh_pos(1));
-            msg.altitude  = llh_pos(2);
-
-            // Populate LLH covariance
-            const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3);
-            const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position);
-            const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose();
-            cov_map = p_cov_l;
-            msg.position_covariance_type = 3;
+        fpmsgs::NmeaZda msg;
+        msg.talker = NmeaTalkerIdToMsg(msg, payload.talker);
+        if (payload.date.valid) {
+            msg.date_valid = true;
+            msg.date_y = payload.date.years;
+            msg.date_m = payload.date.months;
+            msg.date_d = payload.date.days;
         }
-
-        // Populate LLH status
-        int status_flag = std::max(data.gnss1_status, data.gnss2_status);
-
-        if (status_flag < static_cast<int8_t>(GnssStatus::FIX_TYPE_S2D)) {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_NO_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_NONE);
-
-        } else if (status_flag >= static_cast<int8_t>(GnssStatus::FIX_TYPE_S2D) || status_flag < static_cast<int8_t>(GnssStatus::FIX_TYPE_RTK_FLOAT)) {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_ALL);
-
-        } else if (status_flag >= static_cast<int8_t>(GnssStatus::FIX_TYPE_RTK_FLOAT)) {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_GBAS_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_ALL);
-
-        } else {
-            msg.status.status = static_cast<int8_t>(NavSatStatusData::Status::STATUS_NO_FIX);
-            msg.status.service = static_cast<uint16_t>(NavSatStatusData::Service::SERVICE_NONE);
+        if (payload.time.valid) {
+            msg.time_valid = true;
+            msg.time_h = payload.time.hours;
+            msg.time_m = payload.time.mins;
+            msg.time_s = payload.time.secs;
         }
-
-        // Publish message
+        msg.local_hr = payload.local_hr.value;
+        msg.local_min = payload.local_min.value;
         pub->publish(msg);
     }
 }
 
-void OdomToImuMsg(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        sensor_msgs::msg::Imu msg;
-    
-        // Populate message
-        if (data.odom.stamp.tow == 0.0 && data.odom.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.odom.stamp);
-        }
-        
-        msg.header.frame_id = data.odom.frame_id;
-        tf2::toMsg(data.acceleration, msg.linear_acceleration);
-        tf2::toMsg(data.odom.twist.angular, msg.angular_velocity);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
-        pub->publish(msg);
+void PublishParserMsg(const fpsdk::common::parser::ParserMsg& msg,
+                      rclcpp::Publisher<fpmsgs::ParserMsg>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::ParserMsg ros_msg;
+        ros_msg.header.stamp = rclcpp::Clock().now();
+        ros_msg.protocol = ParserProtocolToMsg(ros_msg, msg.proto_);
+        ros_msg.data = msg.data_;
+        ros_msg.name = msg.name_;
+        ros_msg.seq = msg.seq_;
+        msg.MakeInfo();
+        ros_msg.info = msg.info_;
+        pub->publish(ros_msg);
     }
 }
 
-void OdomToYprMsg(const OdometryData& data, rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        geometry_msgs::msg::Vector3Stamped msg;
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Populate message
-        if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(data.stamp);
+void PublishNmeaEpochData(const NmeaEpochData& data, rclcpp::Publisher<fpmsgs::NmeaEpoch>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::NmeaEpoch msg;
+        msg.header.stamp = ros2::utils::ConvTime(data.stamp_);
+        msg.header.frame_id = data.frame_id_;
+        if (data.date_.valid) {
+            msg.date_valid = true;
+            msg.date_y = data.date_.years;
+            msg.date_m = data.date_.months;
+            msg.date_d = data.date_.days;
+        }
+        if (data.time_.valid) {
+            msg.time_valid = true;
+            msg.time_h = data.time_.hours;
+            msg.time_m = data.time_.mins;
+            msg.time_s = data.time_.secs;
+        }
+        msg.status = NmeaStatusGllRmcToMsg(msg, data.status_);
+        msg.navstatus = NmeaNavStatusRmcToMsg(msg, data.navstatus_);
+        msg.mode1 = NmeaModeRmcGnsToMsg(msg, data.mode1_);
+        msg.mode2 = NmeaModeGllVtgToMsg(msg, data.mode2_);
+        msg.quality = NmeaQualityGgaToMsg(msg, data.quality_);
+        msg.opmode = NmeaOpModeGsaToMsg(msg, data.opmode_);
+        msg.navmode = NmeaNavModeGsaToMsg(msg, data.navmode_);
+        msg.latitude = (data.llh_.latlon_valid ? data.llh_.lat : NAN);
+        msg.longitude = (data.llh_.latlon_valid ? data.llh_.lon : NAN);
+        msg.height = (data.llh_.height_valid ? data.llh_.height : NAN);
+        msg.num_sv = (data.num_sv_.valid ? data.num_sv_.value : -1);
+        msg.rms_range = (data.rms_range_.valid ? data.rms_range_.value : NAN);
+        msg.std_major = (data.std_major_.valid ? data.std_major_.value : NAN);
+        msg.std_minor = (data.std_minor_.valid ? data.std_minor_.value : NAN);
+        msg.angle_major = (data.angle_major_.valid ? data.angle_major_.value : NAN);
+        msg.std_lat = (data.std_lat_.valid ? data.std_lat_.value : NAN);
+        msg.std_lon = (data.std_lon_.valid ? data.std_lon_.value : NAN);
+        msg.std_alt = (data.std_alt_.valid ? data.std_alt_.value : NAN);
+        msg.pdop = (data.pdop_.valid ? data.pdop_.value : NAN);
+        msg.hdop = (data.hdop_.valid ? data.hdop_.value : NAN);
+        msg.vdop = (data.vdop_.valid ? data.vdop_.value : NAN);
+        msg.heading = (data.heading_.valid ? data.heading_.value : NAN);
+        msg.speed = (data.speed_.valid ? data.speed_.value : NAN);
+        msg.course = (data.course_.valid ? data.course_.value : NAN);
+        msg.cogt = (data.cogt_.valid ? data.cogt_.value : NAN);
+        msg.cogm = (data.cogm_.valid ? data.cogm_.value : NAN);
+        msg.sogn = (data.sogn_.valid ? data.sogn_.value : NAN);
+        msg.sogk = (data.sogk_.valid ? data.sogk_.value : NAN);
+        msg.diff_age = (data.diff_age_.valid ? data.diff_age_.value : NAN);
+        msg.diff_sta = data.diff_sta_.value;
+        for (auto& sat : data.gsa_gsv_.sats_) {
+            fpmsgs::NmeaSatellite sat_msg;
+            sat_msg.system = NmeaSystemIdToMsg(sat_msg, sat.system_);
+            sat_msg.svid = sat.svid_;
+            sat_msg.az = sat.az_;
+            sat_msg.el = sat.el_;
+            msg.sats.push_back(sat_msg);
+        }
+        for (auto& sig : data.gsa_gsv_.sigs_) {
+            fpmsgs::NmeaSignal sig_msg;
+            sig_msg.system = NmeaSystemIdToMsg(sig_msg, sig.system_);
+            sig_msg.svid = sig.svid_;
+            sig_msg.signal = NmeaSignalIdToMsg(msg, sig.signal_);
+            sig_msg.cno = sig.cno_;
+            sig_msg.used = sig.used_;
+            msg.sigs.push_back(sig_msg);
         }
-        msg.header.frame_id = "FP_ENU";
 
-        // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll
-        Eigen::Vector3d enu_euler = RotToEul(data.pose.orientation.toRotationMatrix());
-        msg.vector.set__x(enu_euler.x());
-        msg.vector.set__y(enu_euler.y());
-        msg.vector.set__z(enu_euler.z());
+        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map = Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.cov_enu.data());
+        cov_map = data.cov_enu_;
 
-        // Publish message
         pub->publish(msg);
     }
 }
 
-void JumpWarningMsg(std::shared_ptr<rclcpp::Node> node, const times::GpsTime& stamp, const Eigen::Vector3d& pos_diff, 
-                    const Eigen::MatrixXd& prev_cov, rclcpp::Publisher<fixposition_driver_ros2::msg::COVWARN>::SharedPtr pub) {
-    if (pub->get_subscription_count() > 0) {
-        // Create message
-        fixposition_driver_ros2::msg::COVWARN msg;
-
-        // Populate message
-        if (stamp.tow == 0.0 && stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(stamp);
-        }
+// ---------------------------------------------------------------------------------------------------------------------
 
-        std::stringstream warn_msg;
-        warn_msg << "Position jump detected! The change in position is greater than the estimated covariances. "
-                 << "Position difference: [" << pos_diff[0] << ", " << pos_diff[1] << ", " << pos_diff[2] << "], "
-                 << "Covariances: [" << prev_cov(0,0) << ", " << prev_cov(1,1) << ", " << prev_cov(2,2) << "]";
+void PublishOdometryData(const OdometryData& data, rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        nav_msgs::msg::Odometry msg;
+        msg.header.stamp = ros2::utils::ConvTime(data.stamp);
+        msg.header.frame_id = data.frame_id;
+        msg.child_frame_id = data.child_frame_id;
+        PoseWithCovDataToMsg(data.pose, msg.pose);
+        TwistWithCovDataToMsg(data.twist, msg.twist);
+        pub->publish(msg);
+    }
+}
 
-        RCLCPP_WARN(node->get_logger(), "%s", warn_msg.str().c_str());
-        tf2::toMsg(pos_diff, msg.jump);
-        tf2::toMsg(Eigen::Vector3d(prev_cov(0,0),prev_cov(1,1),prev_cov(2,2)), msg.covariance);
+// ---------------------------------------------------------------------------------------------------------------------
 
-        // Publish message
+void PublishJumpWarning(const JumpDetector& jump_detector, rclcpp::Publisher<fpmsgs::CovWarn>::SharedPtr& pub) {
+    if (pub->get_subscription_count() > 0) {
+        fpmsgs::CovWarn msg;
+        msg.header.stamp = ros2::utils::ConvTime(jump_detector.curr_stamp_);
+        tf2::toMsg(jump_detector.pos_diff_, msg.jump);
+        msg.covariance.x = jump_detector.prev_cov_(0, 0);
+        msg.covariance.y = jump_detector.prev_cov_(1, 1);
+        msg.covariance.z = jump_detector.prev_cov_(2, 2);
         pub->publish(msg);
     }
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
index b35c2d2..7d293bc 100644
--- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -1,489 +1,698 @@
 /**
- *  @file
- *  @brief Main function for the fixposition driver ros node
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Fixposition driver node for ROS2
  */
 
+/* LIBC/STL */
+#include <chrono>
+#include <cstdlib>
+#include <cstring>
+#include <functional>
+#include <future>
+#include <memory>
+#include <vector>
+
+/* EXTERNAL */
+#include <fpsdk_common/app.hpp>
+#include <fpsdk_common/logging.hpp>
+#include <fpsdk_common/parser/fpa.hpp>
+#include <fpsdk_common/parser/nmea.hpp>
+#include <fpsdk_common/parser/novb.hpp>
+#include <fpsdk_common/trafo.hpp>
+#include <fpsdk_common/types.hpp>
+#include <fpsdk_ros2/utils.hpp>
+
 /* PACKAGE */
-#include <fixposition_driver_ros2/fixposition_driver_node.hpp>
+#include "fixposition_driver_ros2/fixposition_driver_node.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node, const FixpositionDriverParams& params, rclcpp::QoS qos_settings)
-    : FixpositionDriver(params),
-      node_(node),
-
-      // FP_A messages
-      fpa_odometry_pub_(node_->create_publisher<fixposition_driver_ros2::msg::ODOMETRY>("/fixposition/fpa/odometry", qos_settings)),
-      fpa_imubias_pub_(node_->create_publisher<fixposition_driver_ros2::msg::IMUBIAS>("/fixposition/fpa/imubias", qos_settings)),
-      fpa_eoe_pub_(node_->create_publisher<fixposition_driver_ros2::msg::EOE>("/fixposition/fpa/eoe", qos_settings)),
-      fpa_llh_pub_(node_->create_publisher<fixposition_driver_ros2::msg::LLH>("/fixposition/fpa/llh", qos_settings)),
-      fpa_odomenu_pub_(node_->create_publisher<fixposition_driver_ros2::msg::ODOMENU>("/fixposition/fpa/odomenu", qos_settings)),
-      fpa_odomsh_pub_(node_->create_publisher<fixposition_driver_ros2::msg::ODOMSH>("/fixposition/fpa/odomsh", qos_settings)),
-      fpa_odomstatus_pub_(node_->create_publisher<fixposition_driver_ros2::msg::ODOMSTATUS>("/fixposition/fpa/odomstatus", qos_settings)),
-      fpa_gnssant_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GNSSANT>("/fixposition/fpa/gnssant", qos_settings)),
-      fpa_gnsscorr_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GNSSCORR>("/fixposition/fpa/gnsscorr", qos_settings)),
-      fpa_text_pub_(node_->create_publisher<fixposition_driver_ros2::msg::TEXT>("/fixposition/fpa/text", qos_settings)),
-      fpa_tp_pub_(node_->create_publisher<fixposition_driver_ros2::msg::TP>("/fixposition/fpa/tp", qos_settings)),
-
-      // NMEA messages
-      nmea_gpgga_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GPGGA>("/fixposition/nmea/gpgga", qos_settings)),
-      nmea_gpgll_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GPGLL>("/fixposition/nmea/gpgll", qos_settings)),
-      nmea_gngsa_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GNGSA>("/fixposition/nmea/gngsa", qos_settings)),
-      nmea_gpgst_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GPGST>("/fixposition/nmea/gpgst", qos_settings)),
-      nmea_gxgsv_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GXGSV>("/fixposition/nmea/gxgsv", qos_settings)),
-      nmea_gphdt_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GPHDT>("/fixposition/nmea/gphdt", qos_settings)),
-      nmea_gprmc_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GPRMC>("/fixposition/nmea/gprmc", qos_settings)),
-      nmea_gpvtg_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GPVTG>("/fixposition/nmea/gpvtg", qos_settings)),
-      nmea_gpzda_pub_(node_->create_publisher<fixposition_driver_ros2::msg::GPZDA>("/fixposition/nmea/gpzda", qos_settings)),
-
-      // ODOMETRY
-      odometry_ecef_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry_ecef", qos_settings)),
-      odometry_llh_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/odometry_llh", qos_settings)),
-      odometry_enu_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry_enu", qos_settings)),
-      odometry_smooth_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry_smooth", qos_settings)),
-
-      // Orientation
-      eul_pub_(node_->create_publisher<geometry_msgs::msg::Vector3Stamped>("/fixposition/ypr", qos_settings)),
-      eul_imu_pub_(node_->create_publisher<geometry_msgs::msg::Vector3Stamped>("/fixposition/imu_ypr", qos_settings)),
-
-      // IMU
-      rawimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/rawimu", qos_settings)),
-      corrimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/corrimu", qos_settings)),
-      poiimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/poiimu", qos_settings)),
-
-      // GNSS
-      nmea_pub_(node_->create_publisher<fixposition_driver_ros2::msg::NMEA>("/fixposition/nmea", qos_settings)),
-      navsatfix_gnss1_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gnss1", qos_settings)),
-      navsatfix_gnss2_pub_(node_->create_publisher<sensor_msgs::msg::NavSatFix>("/fixposition/gnss2", qos_settings)),
-
-      // TF
-      br_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
-      static_br_(std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_))
+using namespace fpsdk::common;
+using namespace fpsdk::common::parser;
+
+FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> nh, const DriverParams& driver_params,
+                                             const NodeParams& node_params) /* clang-format off */ :
+    nh_                { nh },
+    driver_params_     { driver_params },
+    node_params_       { node_params },
+    logger_            { nh_->get_logger() },
+    driver_            { driver_params },
+    qos_settings_      { rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default) },
+    nmea_epoch_data_   { driver_params_.nmea_epoch_ }  // clang-format on
 
 {
-    ws_sub_ = node_->create_subscription<fixposition_driver_ros2::msg::Speed>(
-        params_.customer_input.speed_topic, 100,
-        std::bind(&FixpositionDriverNode::WsCallbackRos, this, std::placeholders::_1));
-    rtcm_sub_ = node_->create_subscription<rtcm_msgs::msg::Message>(
-        params_.customer_input.rtcm_topic, 10,
-        std::bind(&FixpositionDriverNode::RtcmCallbackRos, this, std::placeholders::_1));
-
-    // Configure jump warning message
-    if (params_.fp_output.cov_warning) {
-        extras_jump_pub_ = node_->create_publisher<fixposition_driver_ros2::msg::COVWARN>("/fixposition/extras/jump", qos_settings);
-        prev_pos.setZero();
-        prev_cov.setZero();
-    }
-    RegisterObservers();
+    // Override default QoS settings
+    // - Short-queue sensor-type QoS
+    if (node_params.qos_type_ == "sensor_short") {
+        qos_settings_ = rclcpp::QoS(rclcpp::KeepLast(5), rmw_qos_profile_sensor_data);
+    }
+    // - Long-queue sensor-type QoS
+    else if (node_params.qos_type_ == "sensor_long") {
+        qos_settings_ = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_sensor_data);
+    }
+    // - Short-queue default-type QoS
+    else if (node_params.qos_type_ == "default_short") {
+        qos_settings_ = rclcpp::QoS(rclcpp::KeepLast(5), rmw_qos_profile_default);
+    }
+    // - Long-queue default-type QoS
+    else if (node_params.qos_type_ == "default_long") {
+        qos_settings_ = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default);
+    }
 }
 
-void FixpositionDriverNode::Run() {
-    rclcpp::Rate rate(params_.fp_output.rate);
-    const auto reconnect_delay =
-        std::chrono::nanoseconds((uint64_t)params_.fp_output.reconnect_delay * 1000 * 1000 * 1000);
-
-    while (rclcpp::ok()) {
-        // Read data and publish to ros
-        const bool connection_ok = RunOnce();
-        // process Incoming ROS msgs
-        rclcpp::spin_some(node_);
-        // Handle connection loss
-        if (!connection_ok) {
-            printf("Reconnecting in %.1f seconds ...\n", params_.fp_output.reconnect_delay);
-            rclcpp::sleep_for(reconnect_delay);
-            Connect();
-        } else {
-            rate.sleep();  // ensure the loop runs at the desired rate
-        }
+FixpositionDriverNode::~FixpositionDriverNode() {}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+// Helper for advertising output topics
+#define _PUB(_pub_, _type_, _topic_, ...)                                      \
+    do {                                                                       \
+        RCLCPP_INFO(logger_, "Advertise %s (" #_type_ ")", (_topic_).c_str()); \
+        _pub_ = nh_->create_publisher<_type_>(_topic_, __VA_ARGS__);           \
+    } while (0)
+
+// Helper for subscribing to input topics
+#define _SUB(_sub_, _type_, _topic_, ...)                                      \
+    do {                                                                       \
+        RCLCPP_INFO(logger_, "Subscribe %s (" #_type_ ")", (_topic_).c_str()); \
+        _sub_ = nh_->create_subscription<_type_>(_topic_, __VA_ARGS__);        \
+    } while (0)
+
+bool FixpositionDriverNode::StartNode() {
+    RCLCPP_INFO(logger_, "Starting...");
+
+    // TF
+    tf_br_ = std::make_unique<tf2_ros::TransformBroadcaster>(nh_);
+    static_br_ = std::make_unique<tf2_ros::StaticTransformBroadcaster>(nh_);
+
+    // Add observers and advertise output topics, depending on configuration
+    const std::string output_ns = (node_params_.output_ns_.empty() ? nh_->get_namespace() : node_params_.output_ns_);
+
+    // FP_A-ODOMETRY
+    if (driver_params_.MessageEnabled(fpa::FpaOdometryPayload::MSG_NAME)) {
+        _PUB(fpa_odometry_pub_, fpmsgs::FpaOdometry, output_ns + "/fpa/odometry", 5);
+        _PUB(odometry_ecef_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_ecef", 5);
+        _PUB(odometry_llh_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/odometry_llh", 5);
+        _PUB(poiimu_pub_, sensor_msgs::msg::Imu, output_ns + "/poiimu", 5);
+        driver_.AddFpaObserver(fpa::FpaOdometryPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto odometry_payload = dynamic_cast<const fpa::FpaOdometryPayload&>(payload);
+            PublishFpaOdometry(odometry_payload, fpa_odometry_pub_);
+            PublishFpaOdometryDataImu(odometry_payload, poiimu_pub_);
+            PublishFpaOdometryDataNavSatFix(odometry_payload, odometry_llh_pub_);
+            OdometryData odometry_data;
+            odometry_data.SetFromFpaOdomPayload(odometry_payload);
+            PublishOdometryData(odometry_data, odometry_ecef_pub_);
+            // ProcessOdometryData(odometry_data);
+        });
     }
-}
 
-void FixpositionDriverNode::RegisterObservers() {
-    // NOV_B
-    bestgnsspos_obs_.push_back(std::bind(&FixpositionDriverNode::BestGnssPosToPublishNavSatFix, this,
-                                         std::placeholders::_1, std::placeholders::_2));
-    // FP_A
-    for (const auto& format : params_.fp_output.formats) {
-        if (format == "ODOMETRY") {
-            dynamic_cast<NmeaConverter<FP_ODOMETRY>*>(a_converters_["ODOMETRY"].get())
-                ->AddObserver([this](const FP_ODOMETRY& data) {
-                    FpToRosMsg(data, fpa_odometry_pub_);
-                    FpToRosMsg(data.odom, odometry_ecef_pub_);
-                    OdomToImuMsg(data, poiimu_pub_);
-                    OdomToNavSatFix(data, odometry_llh_pub_);
-                    OdometryDataToTf(data, br_);
-
-                    // Output jump warning
-                    if (params_.fp_output.cov_warning) {
-                        if (!prev_pos.isZero() && !prev_cov.isZero()) {
-                            Eigen::Vector3d pos_diff = (prev_pos - data.odom.pose.position).cwiseAbs();
-
-                            if ((pos_diff[0] > prev_cov(0,0)) || (pos_diff[1] > prev_cov(1,1)) || (pos_diff[2] > prev_cov(2,2))) {
-                                JumpWarningMsg(node_, data.odom.stamp, pos_diff, prev_cov, extras_jump_pub_);
-                            }
-                        }
-                        prev_pos = data.odom.pose.position;
-                        prev_cov = data.odom.pose.cov;
-                    }
-                });
-        } else if (format == "ODOMENU") {
-            dynamic_cast<NmeaConverter<FP_ODOMENU>*>(a_converters_["ODOMENU"].get())
-                ->AddObserver([this](const FP_ODOMENU& data) {
-                    FpToRosMsg(data, fpa_odomenu_pub_);
-                    FpToRosMsg(data.odom, odometry_enu_pub_);
-                    OdomToYprMsg(data.odom, eul_pub_);
-
-                    // Append TF if Nav2 mode is selected
-                    if (params_.fp_output.nav2_mode) {
-                        // Get FP_ENU0 -> FP_POI
-                        geometry_msgs::msg::TransformStamped tf;
-                        OdomToTf(data.odom, tf);
-                        tf_map["ENU0POI"] = std::make_shared<geometry_msgs::msg::TransformStamped>(tf);
-                    }
-                });
-        } else if (format == "ODOMSH") {
-            dynamic_cast<NmeaConverter<FP_ODOMSH>*>(a_converters_["ODOMSH"].get())
-                ->AddObserver([this](const FP_ODOMSH& data) {
-                    FpToRosMsg(data, fpa_odomsh_pub_);
-                    FpToRosMsg(data.odom, odometry_smooth_pub_);
-
-                    // Append TF if Nav2 mode is selected
-                    if (params_.fp_output.nav2_mode) {
-                        // Get FP_ECEF -> FP_POISH
-                        geometry_msgs::msg::TransformStamped tf;
-                        OdomToTf(data.odom, tf);
-                        tf_map["ECEFPOISH"] = std::make_shared<geometry_msgs::msg::TransformStamped>(tf);
-                    }
-                });
-        } else if (format == "ODOMSTATUS") {
-            dynamic_cast<NmeaConverter<FP_ODOMSTATUS>*>(a_converters_["ODOMSTATUS"].get())
-                ->AddObserver([this](const FP_ODOMSTATUS& data) { FpToRosMsg(data, fpa_odomstatus_pub_); });
-        } else if (format == "IMUBIAS") {
-            dynamic_cast<NmeaConverter<FP_IMUBIAS>*>(a_converters_["IMUBIAS"].get())
-                ->AddObserver([this](const FP_IMUBIAS& data) { FpToRosMsg(data, fpa_imubias_pub_); });
-        } else if (format == "EOE") {
-            dynamic_cast<NmeaConverter<FP_EOE>*>(a_converters_["EOE"].get())
-                ->AddObserver([this](const FP_EOE& data) {
-                    FpToRosMsg(data, fpa_eoe_pub_);
-
-                    // Generate Nav2 TF tree
-                    if (data.epoch == "FUSION" && params_.fp_output.nav2_mode) {
-                        PublishNav2Tf(tf_map, static_br_, br_);
-                    }
-                });
-        } else if (format == "LLH") {
-            dynamic_cast<NmeaConverter<FP_LLH>*>(a_converters_["LLH"].get())
-                ->AddObserver([this](const FP_LLH& data) { FpToRosMsg(data, fpa_llh_pub_); });
-        } else if (format == "GNSSANT") {
-            dynamic_cast<NmeaConverter<FP_GNSSANT>*>(a_converters_["GNSSANT"].get())
-                ->AddObserver([this](const FP_GNSSANT& data) { FpToRosMsg(data, fpa_gnssant_pub_); });
-        } else if (format == "GNSSCORR") {
-            dynamic_cast<NmeaConverter<FP_GNSSCORR>*>(a_converters_["GNSSCORR"].get())
-                ->AddObserver([this](const FP_GNSSCORR& data) { FpToRosMsg(data, fpa_gnsscorr_pub_); });
-        } else if (format == "TEXT") {
-            dynamic_cast<NmeaConverter<FP_TEXT>*>(a_converters_["TEXT"].get())
-                ->AddObserver([this](const FP_TEXT& data) { FpToRosMsg(data, fpa_text_pub_); });
-        } else if (format == "RAWIMU") {
-            dynamic_cast<NmeaConverter<FP_RAWIMU>*>(a_converters_["RAWIMU"].get())
-                ->AddObserver([this](const FP_RAWIMU& data) { FpToRosMsg(data.imu, rawimu_pub_); });
-        } else if (format == "CORRIMU") {
-            dynamic_cast<NmeaConverter<FP_CORRIMU>*>(a_converters_["CORRIMU"].get())
-                ->AddObserver([this](const FP_CORRIMU& data) { FpToRosMsg(data.imu, corrimu_pub_); });
-        } else if (format == "TF") {
-            dynamic_cast<NmeaConverter<FP_TF>*>(a_converters_["TF"].get())->AddObserver([this](const FP_TF& data) {
-                if (data.valid_tf) {
-                    // TF Observer Lambda
-                    geometry_msgs::msg::TransformStamped tf;
-                    TfDataToMsg(data.tf, tf);
-                    if (tf.child_frame_id == "FP_IMUH" && tf.header.frame_id == "FP_POI") {
-                        br_->sendTransform(tf);
-
-                        // Publish Pitch Roll based on IMU only
-                        Eigen::Vector3d imu_ypr_eigen = QuatToEul(data.tf.rotation);
-                        imu_ypr_eigen.x() = 0.0;  // the yaw value is not observable using IMU alone
-                        geometry_msgs::msg::Vector3Stamped imu_ypr;
-                        imu_ypr.header.stamp = tf.header.stamp;
-                        imu_ypr.header.frame_id = "FP_POI";
-                        imu_ypr.vector.set__x(imu_ypr_eigen.x());
-                        imu_ypr.vector.set__y(imu_ypr_eigen.y());
-                        imu_ypr.vector.set__z(imu_ypr_eigen.z());
-                        eul_imu_pub_->publish(imu_ypr);
-
-                    } else if (tf.child_frame_id == "FP_POISH" && tf.header.frame_id == "FP_POI") {
-                        br_->sendTransform(tf);
-
-                        // Append TF if Nav2 mode is selected
-                        if (params_.fp_output.nav2_mode) {
-                            // Get FP_POI -> FP_POISH
-                            tf_map["POIPOISH"] = std::make_shared<geometry_msgs::msg::TransformStamped>(tf);
-                        }
-                    } else if (tf.child_frame_id == "FP_ENU0" && tf.header.frame_id == "FP_ECEF") {
-                        static_br_->sendTransform(tf);
-
-                        // Append TF if Nav2 mode is selected
-                        if (params_.fp_output.nav2_mode) {
-                            // Get FP_ECEF -> FP_ENU0
-                            tf_map["ECEFENU0"] = std::make_shared<geometry_msgs::msg::TransformStamped>(tf);
-                        }
-                    } else {
-                        static_br_->sendTransform(tf);
-                    }
-                }
-            });
-        } else if (format == "TP") {
-            dynamic_cast<NmeaConverter<FP_TP>*>(a_converters_["TP"].get())
-                ->AddObserver([this](const FP_TP& data) { FpToRosMsg(data, fpa_tp_pub_); });
-        } else if (format == "GPGGA") {
-            dynamic_cast<NmeaConverter<GP_GGA>*>(a_converters_["GPGGA"].get())->AddObserver([this](const GP_GGA& data) {
-                FpToRosMsg(data, nmea_gpgga_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) {
-                    nmea_message_.AddNmeaEpoch(data);
-                    PublishNmea();  // GPGGA controls the NMEA output
-                }
-            });
-        } else if (format == "GPGLL") {
-            dynamic_cast<NmeaConverter<GP_GLL>*>(a_converters_["GPGLL"].get())->AddObserver([this](const GP_GLL& data) {
-                FpToRosMsg(data, nmea_gpgll_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GNGSA") {
-            dynamic_cast<NmeaConverter<GN_GSA>*>(a_converters_["GNGSA"].get())->AddObserver([this](const GN_GSA& data) {
-                FpToRosMsg(data, nmea_gngsa_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPGST") {
-            dynamic_cast<NmeaConverter<GP_GST>*>(a_converters_["GPGST"].get())->AddObserver([this](const GP_GST& data) {
-                FpToRosMsg(data, nmea_gpgst_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GXGSV") {
-            dynamic_cast<NmeaConverter<GX_GSV>*>(a_converters_["GXGSV"].get())->AddObserver([this](const GX_GSV& data) {
-                FpToRosMsg(data, nmea_gxgsv_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPHDT") {
-            dynamic_cast<NmeaConverter<GP_HDT>*>(a_converters_["GPHDT"].get())->AddObserver([this](const GP_HDT& data) {
-                FpToRosMsg(data, nmea_gphdt_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPRMC") {
-            dynamic_cast<NmeaConverter<GP_RMC>*>(a_converters_["GPRMC"].get())->AddObserver([this](const GP_RMC& data) {
-                FpToRosMsg(data, nmea_gprmc_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPVTG") {
-            dynamic_cast<NmeaConverter<GP_VTG>*>(a_converters_["GPVTG"].get())->AddObserver([this](const GP_VTG& data) {
-                FpToRosMsg(data, nmea_gpvtg_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        } else if (format == "GPZDA") {
-            dynamic_cast<NmeaConverter<GP_ZDA>*>(a_converters_["GPZDA"].get())->AddObserver([this](const GP_ZDA& data) {
-                FpToRosMsg(data, nmea_gpzda_pub_);
-                if (nmea_pub_->get_subscription_count() > 0) nmea_message_.AddNmeaEpoch(data);
-            });
-        }
+    // FP_A-ODOMSH
+    if (driver_params_.MessageEnabled(fpa::FpaOdomshPayload::MSG_NAME)) {
+        _PUB(fpa_odomsh_pub_, fpmsgs::FpaOdomsh, output_ns + "/fpa/odomsh", 5);
+        _PUB(odometry_smooth_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_smooth", 5);
+        driver_.AddFpaObserver(fpa::FpaOdomshPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto odomsh_payload = dynamic_cast<const fpa::FpaOdomshPayload&>(payload);
+            PublishFpaOdomsh(odomsh_payload, fpa_odomsh_pub_);
+            OdometryData odometry_data;
+            odometry_data.SetFromFpaOdomPayload(odomsh_payload);
+            PublishOdometryData(odometry_data, odometry_smooth_pub_);
+            // ProcessOdometryData(odometry_data);
+        });
     }
-}
 
-void FixpositionDriverNode::PublishNmea() {
-    // If epoch message is complete, generate NMEA output
-    if (nmea_message_.checkEpoch()) {
-        // Generate new message
-        fixposition_driver_ros2::msg::NMEA msg;
+    // FP_A-ODOMENU
+    if (driver_params_.MessageEnabled(fpa::FpaOdomenuPayload::MSG_NAME)) {
+        _PUB(fpa_odomenu_pub_, fpmsgs::FpaOdomenu, output_ns + "/fpa/odomenu", 5);
+        _PUB(odometry_enu_pub_, nav_msgs::msg::Odometry, output_ns + "/odometry_enu", 5);
+        _PUB(eul_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/ypr", 5);
+        driver_.AddFpaObserver(fpa::FpaOdomenuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto odomenu_payload = dynamic_cast<const fpa::FpaOdomenuPayload&>(payload);
+            PublishFpaOdomenu(odomenu_payload, fpa_odomenu_pub_);
+            PublishFpaOdomenuVector3Stamped(odomenu_payload, eul_pub_);
+            OdometryData odometry_data;
+            odometry_data.SetFromFpaOdomPayload(odomenu_payload);
+            PublishOdometryData(odometry_data, odometry_enu_pub_);
+            // ProcessOdometryData(odometry_data);
+        });
+    }
 
-        // ROS Header
-        if (nmea_message_.stamp.tow == 0.0 && nmea_message_.stamp.wno == 0) {
-            msg.header.stamp = rclcpp::Clock().now();
-        } else {
-            msg.header.stamp = GpsTimeToMsgTime(nmea_message_.stamp);
-        }
-        msg.header.frame_id = "FP_POI";
+    // FP_A-ODOMSTATUS
+    if (driver_params_.MessageEnabled(fpa::FpaOdomstatusPayload::MSG_NAME)) {
+        _PUB(fpa_odomstatus_pub_, fpmsgs::FpaOdomstatus, output_ns + "/fpa/odomstatus", 5);
+        driver_.AddFpaObserver(fpa::FpaOdomstatusPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaOdomstatus(dynamic_cast<const fpa::FpaOdomstatusPayload&>(payload), fpa_odomstatus_pub_);
+        });
+    }
 
-        // Time and date fields
-        msg.time = nmea_message_.time_str;
-        msg.date = nmea_message_.date_str;
+    // FP_A-EOE
+    if (driver_params_.MessageEnabled(fpa::FpaEoePayload::MSG_NAME)) {
+        _PUB(fpa_eoe_pub_, fpmsgs::FpaEoe, output_ns + "/fpa/eoe", 5);
+        driver_.AddFpaObserver(fpa::FpaEoePayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            auto eoe_payload = dynamic_cast<const fpa::FpaEoePayload&>(payload);
+            (void)eoe_payload;
+            PublishFpaEoe(eoe_payload, fpa_eoe_pub_);
+            // Generate Nav2 TF tree
+            if (driver_params_.nav2_mode_ && (eoe_payload.epoch == fpa::FpaEpoch::FUSION)) {
+                PublishNav2Tf();
+            }
+            // NMEA epoch
+            if (driver_params_.nmea_epoch_ == eoe_payload.epoch) {
+                PublishNmeaEpochData(nmea_epoch_data_.CompleteAndReset(), nmea_epoch_pub_);
+            }
+        });
+    }
 
-        // Latitude [degrees]. Positive is north of equator; negative is south
-        msg.latitude = nmea_message_.llh(0);
+    // FP_A-TF
+    if (driver_params_.MessageEnabled(fpa::FpaTfPayload::MSG_NAME)) {
+        _PUB(eul_imu_pub_, geometry_msgs::msg::Vector3Stamped, output_ns + "/imu_ypr", 5);
+        driver_.AddFpaObserver(fpa::FpaTfPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            (void)payload;
+            TfData tf;
+            tf.SetFromFpaTfPayload(dynamic_cast<const fpa::FpaTfPayload&>(payload));
+            ProcessTfData(tf);
+        });
+    }
 
-        // Longitude [degrees]. Positive is east of prime meridian; negative is west
-        msg.longitude = nmea_message_.llh(1);
+    // FP_A-LLH
+    if (driver_params_.MessageEnabled(fpa::FpaLlhPayload::MSG_NAME)) {
+        _PUB(fpa_llh_pub_, fpmsgs::FpaLlh, output_ns + "/fpa/llh", 5);
+        driver_.AddFpaObserver(fpa::FpaLlhPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaLlh(dynamic_cast<const fpa::FpaLlhPayload&>(payload), fpa_llh_pub_);
+        });
+    }
 
-        // Altitude [m]. Positive is above the WGS-84 ellipsoid
-        msg.altitude = nmea_message_.llh(2);
+    // FP_A-GNSSANT
+    if (driver_params_.MessageEnabled(fpa::FpaGnssantPayload::MSG_NAME)) {
+        _PUB(fpa_gnssant_pub_, fpmsgs::FpaGnssant, output_ns + "/fpa/gnssant", 5);
+        driver_.AddFpaObserver(fpa::FpaGnssantPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaGnssant(dynamic_cast<const fpa::FpaGnssantPayload&>(payload), fpa_gnssant_pub_);
+        });
+    }
 
-        // Quality indicator
-        msg.quality = nmea_message_.quality;
+    // FP_A-GNSSCORR
+    if (driver_params_.MessageEnabled(fpa::FpaGnsscorrPayload::MSG_NAME)) {
+        _PUB(fpa_gnsscorr_pub_, fpmsgs::FpaGnsscorr, output_ns + "/fpa/gnsscorr", 5);
+        driver_.AddFpaObserver(fpa::FpaGnsscorrPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaGnsscorr(dynamic_cast<const fpa::FpaGnsscorrPayload&>(payload), fpa_gnsscorr_pub_);
+        });
+    }
 
-        // Number of satellites
-        msg.num_sv = nmea_message_.num_sv;
+    // FP_A-IMUBIAS
+    if (driver_params_.MessageEnabled(fpa::FpaImubiasPayload::MSG_NAME)) {
+        _PUB(fpa_imubias_pub_, fpmsgs::FpaImubias, output_ns + "/fpa/imubias", 5);
+        driver_.AddFpaObserver(fpa::FpaImubiasPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaImubias(dynamic_cast<const fpa::FpaImubiasPayload&>(payload), fpa_imubias_pub_);
+        });
+    }
 
-        // ID numbers of satellites used in solution
-        for (unsigned int i = 0; i < nmea_message_.ids.size(); i++) {
-            msg.ids.push_back(nmea_message_.ids.at(i));
-        }
+    // FP_A-RAWIMU
+    if (driver_params_.MessageEnabled(fpa::FpaRawimuPayload::MSG_NAME)) {
+        _PUB(rawimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/rawimu", 5);
+        driver_.AddFpaObserver(fpa::FpaRawimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaRawimu(dynamic_cast<const fpa::FpaRawimuPayload&>(payload), rawimu_pub_);
+        });
+    }
 
-        // Dilution of precision
-        msg.hdop_rec = nmea_message_.hdop_receiver;
-        msg.pdop = nmea_message_.pdop;
-        msg.hdop = nmea_message_.hdop;
-        msg.vdop = nmea_message_.vdop;
-
-        // Populate GNSS pseudorange error statistics
-        msg.rms_range = nmea_message_.rms_range;
-        msg.std_major = nmea_message_.std_major;
-        msg.std_minor = nmea_message_.std_minor;
-        msg.angle_major = nmea_message_.angle_major;
-        msg.std_lat = nmea_message_.std_lat;
-        msg.std_lon = nmea_message_.std_lon;
-        msg.std_alt = nmea_message_.std_alt;
-
-        // Position covariance [m^2]
-        Eigen::Map<Eigen::Matrix<double, 3, 3>> cov_map =
-            Eigen::Map<Eigen::Matrix<double, 3, 3>>(msg.covariance.data());
-        cov_map = nmea_message_.cov;
-
-        // Method employed to estimate covariance
-        msg.cov_type = nmea_message_.cov_type;
-
-        // Populate GNSS satellites in view
-        for (auto gsv_it = nmea_message_.gnss_signals.begin(); gsv_it != nmea_message_.gnss_signals.end(); ++gsv_it) {
-            SignalType msg_type = gsv_it->first;
-            std::map<unsigned int, GnssSignalStats>* gnss_data = &gsv_it->second;
-
-            // Populate GnssSats message
-            fixposition_driver_ros2::msg::Gnsssats sats_msg;
-
-            // Get constellation name
-            if (msg_type == SignalType::GPS) {
-                sats_msg.constellation = "GPS";
-            } else if (msg_type == SignalType::Galileo) {
-                sats_msg.constellation = "Galileo";
-            } else if (msg_type == SignalType::BeiDou) {
-                sats_msg.constellation = "BeiDou";
-            } else if (msg_type == SignalType::GLONASS) {
-                sats_msg.constellation = "GLONASS";
-            } else {
-                sats_msg.constellation = "Unknown";
-            }
+    // FP_A-CORRIMU
+    if (driver_params_.MessageEnabled(fpa::FpaCorrimuPayload::MSG_NAME)) {
+        _PUB(corrimu_pub_, sensor_msgs::msg::Imu, output_ns + "/fpa/corrimu", 5);
+        driver_.AddFpaObserver(fpa::FpaCorrimuPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaCorrimu(dynamic_cast<const fpa::FpaCorrimuPayload&>(payload), corrimu_pub_);
+        });
+    }
 
-            // Get signal statistics
-            for (auto it = gnss_data->begin(); it != gnss_data->end(); ++it) {
-                unsigned int sat_id = it->first;
-                GnssSignalStats signals = it->second;
+    // FP_A-TEXT
+    if (driver_params_.MessageEnabled(fpa::FpaTextPayload::MSG_NAME)) {
+        _PUB(fpa_text_pub_, fpmsgs::FpaText, output_ns + "/fpa/text", 5);
+        driver_.AddFpaObserver(fpa::FpaTextPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaText(dynamic_cast<const fpa::FpaTextPayload&>(payload), fpa_text_pub_);
+        });
+    }
 
-                sats_msg.sat_id.push_back(sat_id);
-                sats_msg.azim.push_back(signals.azim);
-                sats_msg.elev.push_back(signals.elev);
-                sats_msg.cno_l1.push_back(signals.cno_l1);
-                sats_msg.cno_l2.push_back(signals.cno_l2);
-            }
+    // FP_A-TP
+    if (driver_params_.MessageEnabled(fpa::FpaTpPayload::MSG_NAME)) {
+        _PUB(fpa_tp_pub_, fpmsgs::FpaTp, output_ns + "/fpa/tp", 5);
+        driver_.AddFpaObserver(fpa::FpaTpPayload::MSG_NAME, [this](const fpa::FpaPayload& payload) {
+            PublishFpaTp(dynamic_cast<const fpa::FpaTpPayload&>(payload), fpa_tp_pub_);
+        });
+    }
 
-            // Add GnssSats to NMEA message
-            msg.gnss_sats.push_back(sats_msg);
-        }
+    // NOV_B-BESTGNSSPOS
+    if (driver_params_.MessageEnabled(novb::NOV_B_BESTGNSSPOS_STRID)) {
+        _PUB(navsatfix_gnss1_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss1", 5);
+        _PUB(navsatfix_gnss2_pub_, sensor_msgs::msg::NavSatFix, output_ns + "/gnss2", 5);
+        driver_.AddNovbObserver(  //
+            novb::NOV_B_BESTGNSSPOS_STRID, [this](const novb::NovbHeader* header, const uint8_t* payload) {
+                if (!PublishNovbBestgnsspos(header, (novb::NovbBestgnsspos*)payload, navsatfix_gnss1_pub_,
+                                            navsatfix_gnss2_pub_)) {
+                    RCLCPP_WARN_THROTTLE(logger_, *nh_->get_clock(), 1e9, "Bad NOV_B-BESTGNSSPOS");
+                }
+            });
+    }
+
+    // NMEA-GP-GGA
+    if (driver_params_.MessageEnabled(nmea::NmeaGgaPayload::FORMATTER)) {
+        _PUB(nmea_gga_pub_, fpmsgs::NmeaGga, output_ns + "/nmea/gga", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGgaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gga_payload = dynamic_cast<const nmea::NmeaGgaPayload&>(payload);
+            PublishNmeaGga(gga_payload, nmea_gga_pub_);
+            nmea_epoch_data_.gga_ = gga_payload;
+        });
+    }
+
+    // NMEA-GP-GLL
+    if (driver_params_.MessageEnabled(nmea::NmeaGllPayload::FORMATTER)) {
+        _PUB(nmea_gll_pub_, fpmsgs::NmeaGll, output_ns + "/nmea/gll", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGllPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gll_payload = dynamic_cast<const nmea::NmeaGllPayload&>(payload);
+            PublishNmeaGll(gll_payload, nmea_gll_pub_);
+            nmea_epoch_data_.gll_ = gll_payload;
+        });
+    }
 
-        // Clear map
-        nmea_message_.gnss_signals.clear();
+    // NMEA-GN-GSA
+    if (driver_params_.MessageEnabled(nmea::NmeaGsaPayload::FORMATTER)) {
+        _PUB(nmea_gsa_pub_, fpmsgs::NmeaGsa, output_ns + "/nmea/gsa", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGsaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gsa_payload_ = dynamic_cast<const nmea::NmeaGsaPayload&>(payload);
+            PublishNmeaGsa(gsa_payload_, nmea_gsa_pub_);
+            nmea_epoch_data_.gsa_ = gsa_payload_;
+            nmea_epoch_data_.gsa_gsv_.AddGsa(gsa_payload_);
+        });
+    }
 
-        // True heading
-        msg.heading = nmea_message_.heading;
+    // NMEA-GP-GST
+    if (driver_params_.MessageEnabled(nmea::NmeaGstPayload::FORMATTER)) {
+        _PUB(nmea_gst_pub_, fpmsgs::NmeaGst, output_ns + "/nmea/gst", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGstPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gst_payload = dynamic_cast<const nmea::NmeaGstPayload&>(payload);
+            PublishNmeaGst(gst_payload, nmea_gst_pub_);
+            nmea_epoch_data_.gst_ = gst_payload;
+        });
+    }
 
-        // Speed over ground [m/s]
-        msg.speed = nmea_message_.speed;
+    // NMEA-GX-GSV
+    if (driver_params_.MessageEnabled(nmea::NmeaGsvPayload::FORMATTER)) {
+        _PUB(nmea_gsv_pub_, fpmsgs::NmeaGsv, output_ns + "/nmea/gsv", 5);
+        driver_.AddNmeaObserver(nmea::NmeaGsvPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto gsv_payload_ = dynamic_cast<const nmea::NmeaGsvPayload&>(payload);
+            PublishNmeaGsv(gsv_payload_, nmea_gsv_pub_);
+            nmea_epoch_data_.gsa_gsv_.AddGsv(gsv_payload_);
+        });
+    }
 
-        // Course over ground [deg]
-        msg.course = nmea_message_.course;
+    // NMEA-GP-HDT
+    if (driver_params_.MessageEnabled(nmea::NmeaHdtPayload::FORMATTER)) {
+        _PUB(nmea_hdt_pub_, fpmsgs::NmeaHdt, output_ns + "/nmea/hdt", 5);
+        driver_.AddNmeaObserver(nmea::NmeaHdtPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto hdt_payload = dynamic_cast<const nmea::NmeaHdtPayload&>(payload);
+            PublishNmeaHdt(hdt_payload, nmea_hdt_pub_);
+            nmea_epoch_data_.hdt_ = hdt_payload;
+        });
+    }
 
-        // Populate differential data information
-        msg.diff_age = nmea_message_.diff_age;
-        msg.diff_sta = nmea_message_.diff_sta;
+    // NMEA-GP-RMC
+    if (driver_params_.MessageEnabled(nmea::NmeaRmcPayload::FORMATTER)) {
+        _PUB(nmea_rmc_pub_, fpmsgs::NmeaRmc, output_ns + "/nmea/rmc", 5);
+        driver_.AddNmeaObserver(nmea::NmeaRmcPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto rmc_payload = dynamic_cast<const nmea::NmeaRmcPayload&>(payload);
+            PublishNmeaRmc(rmc_payload, nmea_rmc_pub_);
+            nmea_epoch_data_.rmc_ = rmc_payload;
+        });
+    }
 
-        // Publish message
-        nmea_pub_->publish(msg);
+    // NMEA-GP-VTG
+    if (driver_params_.MessageEnabled(nmea::NmeaVtgPayload::FORMATTER)) {
+        _PUB(nmea_vtg_pub_, fpmsgs::NmeaVtg, output_ns + "/nmea/vtg", 5);
+        driver_.AddNmeaObserver(nmea::NmeaVtgPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto vtg_payload = dynamic_cast<const nmea::NmeaVtgPayload&>(payload);
+            PublishNmeaVtg(vtg_payload, nmea_vtg_pub_);
+            nmea_epoch_data_.vtg_ = vtg_payload;
+        });
     }
-}
 
-void FixpositionDriverNode::WsCallbackRos(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) {
-    std::unordered_map<std::string, std::vector<std::pair<bool, int>>> measurements;
-    for (const auto& sensor : msg->sensors) {
-        measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx});
-        measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy});
-        measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz});
+    // NMEA-GP-ZDA
+    if (driver_params_.MessageEnabled(nmea::NmeaZdaPayload::FORMATTER)) {
+        _PUB(nmea_zda_pub_, fpmsgs::NmeaZda, output_ns + "/nmea/zda", 5);
+        driver_.AddNmeaObserver(nmea::NmeaZdaPayload::FORMATTER, [this](const nmea::NmeaPayload& payload) {
+            auto zda_payload = dynamic_cast<const nmea::NmeaZdaPayload&>(payload);
+            PublishNmeaZda(zda_payload, nmea_zda_pub_);
+            nmea_epoch_data_.zda_ = zda_payload;
+        });
     }
-    WsCallback(measurements);
+
+    // Raw messages
+    if (driver_params_.raw_output_) {
+        _PUB(raw_pub_, fpmsgs::ParserMsg, output_ns + "/raw", 5);
+        driver_.AddRawObserver([this](const parser::ParserMsg& msg) { PublishParserMsg(msg, raw_pub_); });
+    }
+
+    // NMEA epoch
+    if (driver_params_.nmea_epoch_ != fpa::FpaEpoch::UNSPECIFIED) {
+        _PUB(nmea_epoch_pub_, fpmsgs::NmeaEpoch, output_ns + "/nmea", 5);
+        // Publish is triggered by FP_A-EOE above
+    }
+
+    // Jump warning message
+    if (driver_params_.cov_warning_) {
+        _PUB(jump_pub_, fpmsgs::CovWarn, output_ns + "/extras/jump", 5);
+    }
+
+    // Subscribe to correction data input
+    if (!node_params_.corr_topic_.empty()) {
+        _SUB(corr_sub_, rtcm_msgs::msg::Message, node_params_.corr_topic_, 100,
+             [this](const rtcm_msgs::msg::Message& msg) {
+                 driver_.SendCorrectionData(msg.message.data(), msg.message.size());
+             });
+    }
+
+    // Subscribe to wheelspeed input
+    if (!node_params_.speed_topic_.empty()) {
+        _SUB(ws_sub_, fpmsgs::Speed, node_params_.speed_topic_, 10,
+             [this](const fpmsgs::Speed& msg) { driver_.SendWheelspeedData(SpeedMsgToWheelspeedData(msg)); });
+    }
+
+    return driver_.StartDriver();
 }
 
-void FixpositionDriverNode::RtcmCallbackRos(const rtcm_msgs::msg::Message::ConstSharedPtr msg) {
-    const void* rtcm_msg = &(msg->message[0]);
-    size_t msg_size = msg->message.size();
-    RtcmCallback(rtcm_msg, msg_size);
+#undef _PUB
+#undef _SUB
+
+void FixpositionDriverNode::StopNode() {
+    RCLCPP_INFO(logger_, "Stopping...");
+
+    driver_.RemoveFpaObservers();
+    driver_.RemoveNmeaObservers();
+    driver_.RemoveNovbObservers();
+
+    driver_.StopDriver();
+
+    // Stop advertising output topics
+    // - FP_A messages
+    fpa_eoe_pub_.reset();
+    fpa_gnssant_pub_.reset();
+    fpa_gnsscorr_pub_.reset();
+    fpa_imubias_pub_.reset();
+    fpa_llh_pub_.reset();
+    fpa_odomenu_pub_.reset();
+    fpa_odometry_pub_.reset();
+    fpa_odomsh_pub_.reset();
+    fpa_odomstatus_pub_.reset();
+    fpa_text_pub_.reset();
+    fpa_tp_pub_.reset();
+    // - NMEA messages
+    nmea_gga_pub_.reset();
+    nmea_gll_pub_.reset();
+    nmea_gsa_pub_.reset();
+    nmea_gst_pub_.reset();
+    nmea_gsv_pub_.reset();
+    nmea_hdt_pub_.reset();
+    nmea_rmc_pub_.reset();
+    nmea_vtg_pub_.reset();
+    nmea_zda_pub_.reset();
+    // - Odometry
+    odometry_ecef_pub_.reset();
+    odometry_enu_pub_.reset();
+    odometry_llh_pub_.reset();
+    odometry_smooth_pub_.reset();
+    // - Orientation
+    eul_pub_.reset();
+    eul_imu_pub_.reset();
+    // - IMU
+    corrimu_pub_.reset();
+    poiimu_pub_.reset();
+    rawimu_pub_.reset();
+    // - GNSS
+    navsatfix_gnss1_pub_.reset();
+    navsatfix_gnss2_pub_.reset();
+    nmea_epoch_pub_.reset();
+    // - Other
+    jump_pub_.reset();
+    raw_pub_.reset();
+
+    // Stop input message subscribers
+    ws_sub_.reset();
+    corr_sub_.reset();
+
+    // TF
+    tf_br_.reset();
+    static_br_.reset();
 }
 
-void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header,
-                                                          const BESTGNSSPOSMem* payload) {
-    // Buffer to data struct
-    NavSatFixData nav_sat_fix;
-    NovToData(header, payload, nav_sat_fix);
-
-    // Publish
-    if (nav_sat_fix.frame_id == "GNSS1" || nav_sat_fix.frame_id == "GNSS") {
-        if (navsatfix_gnss1_pub_->get_subscription_count() > 0) {
-            sensor_msgs::msg::NavSatFix msg;
-            NavSatFixDataToMsg(nav_sat_fix, msg);
-            navsatfix_gnss1_pub_->publish(msg);
+// ---------------------------------------------------------------------------------------------------------------------
+
+void FixpositionDriverNode::ProcessTfData(const TfData& tf_data) {
+    geometry_msgs::msg::TransformStamped tf;
+    TfDataToTransformStamped(tf_data, tf);
+
+    // TODO: use constants from helper.hpp?
+
+    // FP_IMUH -> FP_POI
+    if ((tf.child_frame_id == "FP_IMUH") && (tf.header.frame_id == "FP_POI")) {
+        tf_br_->sendTransform(tf);
+
+        // Publish pitch roll based on IMU only
+        Eigen::Vector3d imu_ypr_eigen = trafo::QuatToEul(tf_data.rotation);
+        imu_ypr_eigen.x() = 0.0;  // the yaw value is not observable using IMU alone
+        geometry_msgs::msg::Vector3Stamped imu_ypr;
+        imu_ypr.header.stamp = tf.header.stamp;
+        imu_ypr.header.frame_id = "FP_POI";
+        imu_ypr.vector.set__x(imu_ypr_eigen.x());
+        imu_ypr.vector.set__y(imu_ypr_eigen.y());
+        imu_ypr.vector.set__z(imu_ypr_eigen.z());
+        eul_imu_pub_->publish(imu_ypr);
+
+    }
+    // FP_POI -> FP_POISH
+    else if ((tf.child_frame_id == "FP_POISH") && (tf.header.frame_id == "FP_POI")) {
+        tf_br_->sendTransform(tf);
+        // Store  TF if Nav2 mode is enabled
+        if (driver_params_.nav2_mode_) {
+            std::unique_lock<std::mutex> lock(tfs_.mutex_);
+            tfs_.poi_poish_ = std::make_unique<geometry_msgs::msg::TransformStamped>(tf);
         }
-    } else if (nav_sat_fix.frame_id == "GNSS2") {
-        if (navsatfix_gnss2_pub_->get_subscription_count() > 0) {
-            sensor_msgs::msg::NavSatFix msg;
-            NavSatFixDataToMsg(nav_sat_fix, msg);
-            navsatfix_gnss2_pub_->publish(msg);
+    }
+    // FP_ECEF -> FP_ENU0
+    else if ((tf.child_frame_id == "FP_ENU0") && (tf.header.frame_id == "FP_ECEF")) {
+        static_br_->sendTransform(tf);
+        // Store TF if Nav2 mode is enabled
+        if (driver_params_.nav2_mode_) {
+            std::unique_lock<std::mutex> lock(tfs_.mutex_);
+            tfs_.ecef_enu0_ = std::make_unique<geometry_msgs::msg::TransformStamped>(tf);
         }
     }
+    // Something else
+    else {
+        static_br_->sendTransform(tf);
+    }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+void FixpositionDriverNode::ProcessOdometryData(const OdometryData& odometry_data) {
+    switch (odometry_data.type) {
+        case OdometryData::Type::ODOMETRY:
+
+            if (odometry_data.valid) {
+                geometry_msgs::msg::TransformStamped tf;
+                OdometryDataToTransformStamped(odometry_data, tf);
+                tf_br_->sendTransform(tf);
+            }
+
+            // Output jump warning
+            if (driver_params_.cov_warning_ && odometry_data.valid && jump_detector_.Check(odometry_data)) {
+                RCLCPP_WARN(logger_, jump_detector_.warning_.c_str());
+                PublishJumpWarning(jump_detector_, jump_pub_);
+            }
+
+            break;
+
+        case OdometryData::Type::ODOMENU:
+            // Store FP_ENU0 -> FP_POI TF if Nav2 mode is selected
+            if (driver_params_.nav2_mode_ && odometry_data.valid) {
+                std::unique_lock<std::mutex> lock(tfs_.mutex_);
+                tfs_.enu0_poi_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+                OdometryDataToTransformStamped(odometry_data, *tfs_.enu0_poi_);
+            }
+            break;
+
+        case OdometryData::Type::ODOMSH:
+            // Store TF if Nav2 mode is selected
+            if (driver_params_.nav2_mode_ && odometry_data.valid) {
+                std::unique_lock<std::mutex> lock(tfs_.mutex_);
+                tfs_.ecef_poish_ = std::make_unique<geometry_msgs::msg::TransformStamped>();
+                OdometryDataToTransformStamped(odometry_data, *tfs_.ecef_poish_);
+            }
+            break;
+
+        case OdometryData::Type::UNSPECIFIED:
+            break;
+    }
+}
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+void FixpositionDriverNode::PublishNav2Tf() {
+    std::unique_lock<std::mutex> lock(tfs_.mutex_);
+    // We'll need all before we can start publishing the Nav2 TFs
+    if (!tfs_.ecef_enu0_ || !tfs_.poi_poish_ || !tfs_.ecef_poish_ || !tfs_.enu0_poi_) {
+        return;
+    }
+
+    // Publish FP_ECEF -> map
+    tfs_.ecef_enu0_->child_frame_id = "map";
+    static_br_->sendTransform(*tfs_.ecef_enu0_);
+
+    // Compute FP_ENU0 -> FP_POISH
+    // Extract translation and rotation from ECEFENU0
+    geometry_msgs::msg::Vector3 trans_ecef_enu0 = tfs_.ecef_enu0_->transform.translation;
+    geometry_msgs::msg::Quaternion rot_ecef_enu0 = tfs_.ecef_enu0_->transform.rotation;
+    Eigen::Vector3d t_ecef_enu0_;
+    t_ecef_enu0_ << trans_ecef_enu0.x, trans_ecef_enu0.y, trans_ecef_enu0.z;
+    Eigen::Quaterniond q_ecef_enu0_(rot_ecef_enu0.w, rot_ecef_enu0.x, rot_ecef_enu0.y, rot_ecef_enu0.z);
+
+    // Extract translation and rotation from ECEFPOISH
+    geometry_msgs::msg::Vector3 trans_ecef_poish = tfs_.ecef_poish_->transform.translation;
+    geometry_msgs::msg::Quaternion rot_ecef_poish = tfs_.ecef_poish_->transform.rotation;
+    Eigen::Vector3d t_ecef_poish;
+    t_ecef_poish << trans_ecef_poish.x, trans_ecef_poish.y, trans_ecef_poish.z;
+    Eigen::Quaterniond q_ecef_poish(rot_ecef_poish.w, rot_ecef_poish.x, rot_ecef_poish.y, rot_ecef_poish.z);
+
+    // Compute the ENU transformation
+    const Eigen::Vector3d t_enu0_poish = trafo::TfEnuEcef(t_ecef_poish, trafo::TfWgs84LlhEcef(t_ecef_enu0_));
+    const Eigen::Quaterniond q_enu0_poish = q_ecef_enu0_.inverse() * q_ecef_poish;
+
+    // Create tf2::Transform tf_ENU0POISH
+    tf2::Transform tf_ENU0POISH;
+    tf_ENU0POISH.setOrigin(tf2::Vector3(t_enu0_poish.x(), t_enu0_poish.y(), t_enu0_poish.z()));
+    tf2::Quaternion tf_q_enu0_poish(q_enu0_poish.x(), q_enu0_poish.y(), q_enu0_poish.z(), q_enu0_poish.w());
+    tf_ENU0POISH.setRotation(tf_q_enu0_poish);
+
+    // Publish map -> odom
+    // Multiply the transforms
+    tf2::Transform tf_ENU0POI;
+    tf2::fromMsg(tfs_.enu0_poi_->transform, tf_ENU0POI);
+    tf2::Transform tf_combined = tf_ENU0POI * tf_ENU0POISH.inverse();
+
+    // Create a new TransformStamped message
+    geometry_msgs::msg::TransformStamped tfs_odom;
+    tfs_odom.header.stamp = nh_->now();
+    tfs_odom.header.frame_id = "map";
+    tfs_odom.child_frame_id = "odom";
+    tfs_odom.transform = tf2::toMsg(tf_combined);
+    tf_br_->sendTransform(tfs_odom);
+
+    // Publish odom -> base_link
+    geometry_msgs::msg::TransformStamped tf_odom_base;
+    tf_odom_base.header.stamp = nh_->now();
+    tf_odom_base.header.frame_id = "odom";
+    tf_odom_base.child_frame_id = "base_link";
+    tf_odom_base.transform = tf2::toMsg(tf_ENU0POISH);
+
+    // Send the transform
+    tf_br_->sendTransform(tf_odom_base);
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
 
+using namespace fixposition;
+
 int main(int argc, char** argv) {
+#ifndef NDEBUG
+    fpsdk::common::app::StacktraceHelper stacktrace;
+    WARNING("***** Running debug build *****");
+#endif
+
+    bool ok = true;
+
+    // Initialise ROS, create node handle
     rclcpp::init(argc, argv);
-    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("fixposition_driver");
-    fixposition::FixpositionDriverParams params;
-
-    RCLCPP_INFO(node->get_logger(), "Starting node...");
-
-    if (fixposition::LoadParamsFromRos2(node, params)) {
-        RCLCPP_INFO(node->get_logger(), "Params Loaded!");
-        
-        // Define ROS QoS
-        rclcpp::QoS qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default);
-
-        if (params.fp_output.qos_type == "sensor_short") { // Short-queue sensor-type QoS
-            qos_settings = rclcpp::QoS(rclcpp::KeepLast(5), rmw_qos_profile_sensor_data);
-        } else if (params.fp_output.qos_type == "sensor_long") { // Long-queue sensor-type QoS
-            qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_sensor_data);
-        } else if (params.fp_output.qos_type == "default_short") { // Short-queue default-type QoS
-            qos_settings = rclcpp::QoS(rclcpp::KeepLast(5), rmw_qos_profile_default);
-        } else if (params.fp_output.qos_type == "default_long") { // Long-queue default-type QoS
-            qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default);
-        } else { // Default QoS profile
-            qos_settings = rclcpp::QoS(rclcpp::KeepLast(10), rmw_qos_profile_default);
+    auto nh = std::make_shared<rclcpp::Node>("fixposition_driver");
+    auto logger = nh->get_logger();
+
+    // Redirect Fixposition SDK logging to ROS console
+    fpsdk::ros2::utils::RedirectLoggingToRosConsole(logger.get_name());
+
+    // Say hello
+    HelloWorld();
+
+    // Load parameters
+    RCLCPP_INFO(logger, "Loading parameters...");
+    DriverParams driver_params;
+    if (!LoadParamsFromRos2(nh, "driver", driver_params)) {
+        RCLCPP_ERROR(logger, "Failed loading sensor params");
+        ok = false;
+    }
+    NodeParams node_params;
+    if (!LoadParamsFromRos2(nh, "node", node_params)) {
+        RCLCPP_ERROR(logger, "Failed loading node params");
+        ok = false;
+    }
+
+    // Handle CTRL-C / SIGINT ourselves
+    fpsdk::common::app::SigIntHelper sigint;
+
+    // Start node
+    std::unique_ptr<FixpositionDriverNode> node;
+    if (ok) {
+        try {
+            node = std::make_unique<FixpositionDriverNode>(nh, driver_params, node_params);
+        } catch (const std::exception& ex) {
+            RCLCPP_ERROR(logger, "Failed creating node: %s", ex.what());
+            ok = false;
+        }
+    }
+    if (ok) {
+        RCLCPP_INFO(logger, "Starting node...");
+        if (node->StartNode()) {
+            RCLCPP_INFO(logger, "main() spinning...");
+
+            // Do the same as rclpp::spin(), but also handle CTRL-C / SIGINT nicely
+            // Callbacks execute in main thread
+            while (rclcpp::ok() && !sigint.ShouldAbort()) {
+                rclcpp::spin_until_future_complete(nh, std::promise<bool>().get_future(),
+                                                   std::chrono::milliseconds(337));
+            }
+
+            // TODO: we'd rather do this, but it (executing in those threads) doesn't seem to work
+            // Use multiple spinner threads. Callback execute in one of them.
+            // rclcpp::executors::MultiThreadedExecutor executor{ rclcpp::ExecutorOptions(), 4 };
+            // executor.add_node(node);
+            // while (rclcpp::ok() && !sigint.ShouldAbort()) {
+            //     executor.spin_once(std::chrono::milliseconds(345));
+            // }
+
+            RCLCPP_INFO(logger, "main() stopping");
+        } else {
+            RCLCPP_ERROR(logger, "Failed starting node");
+            ok = false;
         }
+        node->StopNode();
+        node.reset();
+        nh.reset();
+    }
 
-        fixposition::FixpositionDriverNode driver_node(node, params, qos_settings);
-        driver_node.Run();
-        RCLCPP_INFO(node->get_logger(), "Exiting.");
+    // Are we happy?
+    if (ok) {
+        RCLCPP_INFO(logger, "Done");
     } else {
-        RCLCPP_ERROR(node->get_logger(), "Params Loading Failed!");
-        rclcpp::shutdown();
-        return 1;
+        RCLCPP_FATAL(logger, "Ouch!");
     }
+
+    // Shutdown ROS
+    rclcpp::shutdown();
+
+    exit(ok ? EXIT_SUCCESS : EXIT_FAILURE);
 }
+
+/* ****************************************************************************************************************** */
diff --git a/fixposition_driver_ros2/src/params.cpp b/fixposition_driver_ros2/src/params.cpp
index 90376aa..798a870 100644
--- a/fixposition_driver_ros2/src/params.cpp
+++ b/fixposition_driver_ros2/src/params.cpp
@@ -1,132 +1,137 @@
 /**
- *  @file
- *  @brief Implementation of Parameter Loading
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Implementation of Parameter Loading
  */
 
+/* LIBC/STL */
+#include <cinttypes>
+
+/* EXTERNAL */
+#include <fpsdk_ros2/ext/rclcpp.hpp>
+
 /* PACKAGE */
-#include <fixposition_driver_ros2/params.hpp>
+#include "fixposition_driver_ros2/params.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
-bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node> node, const std::string& ns, FpOutputParams& params) {
-    const std::string RATE = ns + ".rate";
+bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node>& nh, const std::string& ns, DriverParams& params) {
+    auto logger = nh->get_logger();
+    bool ok = true;
+    RCLCPP_INFO(logger, "DriverParams: loading from %s", ns.c_str());
+
+    const std::string STREAM = ns + ".stream";
     const std::string RECONNECT_DELAY = ns + ".reconnect_delay";
-    const std::string TYPE = ns + ".type";
-    const std::string FORMATS = ns + ".formats";
-    const std::string QOS_TYPE = ns + ".qos_type";
-    const std::string IP = ns + ".ip";
-    const std::string PORT = ns + ".port";
-    const std::string BAUDRATE = ns + ".baudrate";
+    const std::string MESSAGES = ns + ".messages";
+    const std::string NMEA_EPOCH = ns + ".nmea_epoch";
+    const std::string RAW_OUTPUT = ns + ".raw_output";
     const std::string COV_WARNING = ns + ".cov_warning";
     const std::string NAV2_MODE = ns + ".nav2_mode";
 
-    node->declare_parameter(RATE, 100);
-    node->declare_parameter(RECONNECT_DELAY, 5.0);
-    node->declare_parameter(TYPE, "tcp");
-    node->declare_parameter(FORMATS, std::vector<std::string>());
-    node->declare_parameter(QOS_TYPE, "sensor_short");
-    node->declare_parameter(PORT, "21000");
-    node->declare_parameter(IP, "127.0.0.1");
-    node->declare_parameter(BAUDRATE, 115200);
-    node->declare_parameter(COV_WARNING, false);
-    node->declare_parameter(NAV2_MODE, false);
-    // read parameters
-    if (node->get_parameter(RATE, params.rate)) {
-        RCLCPP_INFO(node->get_logger(), "%s : %d", RATE.c_str(), params.rate);
-    } else {
-        RCLCPP_WARN(node->get_logger(), "Using Default %s : %d", RATE.c_str(), params.rate);
+    nh->declare_parameter(STREAM, params.stream_);
+    nh->declare_parameter(RECONNECT_DELAY, params.reconnect_delay_);
+    nh->declare_parameter(MESSAGES, params.messages_);
+    nh->declare_parameter(NMEA_EPOCH, "");
+    nh->declare_parameter(RAW_OUTPUT, params.raw_output_);
+    nh->declare_parameter(COV_WARNING, params.cov_warning_);
+    nh->declare_parameter(NAV2_MODE, params.nav2_mode_);
+
+    if (!nh->get_parameter(STREAM, params.stream_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", STREAM.c_str());
+        ok = false;
     }
-    if (node->get_parameter(RECONNECT_DELAY, params.reconnect_delay)) {
-        RCLCPP_INFO(node->get_logger(), "%s : %f", RECONNECT_DELAY.c_str(), params.reconnect_delay);
-    } else {
-        RCLCPP_WARN(node->get_logger(), "%s : %f", RECONNECT_DELAY.c_str(), params.reconnect_delay);
+    if (!nh->get_parameter(RECONNECT_DELAY, params.reconnect_delay_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", RECONNECT_DELAY.c_str());
+        ok = false;
     }
-    if (node->get_parameter(QOS_TYPE, params.qos_type)) {
-        RCLCPP_INFO(node->get_logger(), "%s : %s", QOS_TYPE.c_str(), params.qos_type.c_str());
-    } else {
-        RCLCPP_WARN(node->get_logger(), "%s : %s", QOS_TYPE.c_str(), params.qos_type.c_str());
+    if (!nh->get_parameter(MESSAGES, params.messages_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", MESSAGES.c_str());
+        ok = false;
     }
-    if (node->get_parameter(COV_WARNING, params.cov_warning)) {
-        RCLCPP_INFO(node->get_logger(), "%s : %d", COV_WARNING.c_str(), params.cov_warning);
-    } else {
-        RCLCPP_WARN(node->get_logger(), "%s : %d", COV_WARNING.c_str(), params.cov_warning);
+    std::string epoch_str;
+    if (!nh->get_parameter(NMEA_EPOCH, epoch_str)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", NMEA_EPOCH.c_str());
+        ok = false;
     }
-    if (node->get_parameter(NAV2_MODE, params.nav2_mode)) {
-        RCLCPP_INFO(node->get_logger(), "%s : %d", NAV2_MODE.c_str(), params.nav2_mode);
-    } else {
-        RCLCPP_WARN(node->get_logger(), "%s : %d", NAV2_MODE.c_str(), params.nav2_mode);
+    if (!StrToEpoch(epoch_str, params.nmea_epoch_)) {
+        RCLCPP_WARN(logger, "Bad value for %s param", NMEA_EPOCH.c_str());
+        ok = false;
     }
-
-    std::string type_str;
-    node->get_parameter(TYPE, type_str);
-    if (type_str == "tcp") {
-        params.type = INPUT_TYPE::TCP;
-    } else if (type_str == "serial") {
-        params.type = INPUT_TYPE::SERIAL;
-    } else {
-        RCLCPP_ERROR(node->get_logger(), "Input type has to be tcp or serial!");
-        return false;
+    if (!nh->get_parameter(RAW_OUTPUT, params.raw_output_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", RAW_OUTPUT.c_str());
+        ok = false;
     }
-
-    node->get_parameter(FORMATS, params.formats);
-    for (size_t i = 0; i < params.formats.size(); i++) {
-        RCLCPP_INFO(node->get_logger(), "%s[%ld] : %s", FORMATS.c_str(), i, params.formats.at(i).c_str());
+    if (!nh->get_parameter(COV_WARNING, params.cov_warning_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", COV_WARNING.c_str());
+        ok = false;
     }
-
-    // Get parameters: port (required)
-    if (node->get_parameter(PORT, params.port)) {
-        RCLCPP_INFO(node->get_logger(), "%s : %s", PORT.c_str(), params.port.c_str());
-    } else {
-        RCLCPP_WARN(node->get_logger(), "Using Default %s : %s", PORT.c_str(), params.port.c_str());
+    if (!nh->get_parameter(NAV2_MODE, params.nav2_mode_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", NAV2_MODE.c_str());
+        ok = false;
     }
 
-    if (params.type == INPUT_TYPE::TCP) {
-        if (node->get_parameter(IP, params.ip)) {
-            RCLCPP_INFO(node->get_logger(), "%s : %s", IP.c_str(), params.ip.c_str());
-        } else {
-            RCLCPP_WARN(node->get_logger(), "Using Default %s : %s", IP.c_str(), params.ip.c_str());
-        }
-    } else if (params.type == INPUT_TYPE::SERIAL) {
-        if (node->get_parameter(BAUDRATE, params.baudrate)) {
-            RCLCPP_INFO(node->get_logger(), "%s : %d", BAUDRATE.c_str(), params.baudrate);
-        } else {
-            RCLCPP_WARN(node->get_logger(), "Using Default %s : %d", BAUDRATE.c_str(), params.baudrate);
-        }
+    RCLCPP_INFO(logger, "DriverParams: stream=%s", params.stream_.c_str());
+    RCLCPP_INFO(logger, "DriverParams: reconnect_delay=%.1f", params.reconnect_delay_);
+    for (std::size_t ix = 0; ix < params.messages_.size(); ix++) {
+        RCLCPP_INFO(logger, "DriverParams: messages[%" PRIuMAX "]=%s", ix, params.messages_[ix].c_str());
     }
+    RCLCPP_INFO(logger, "DriverParams: nmea_epoch=%s", epoch_str.c_str());
+    RCLCPP_INFO(logger, "DriverParams: raw_output=%s", params.raw_output_ ? "true" : "false");
+    RCLCPP_INFO(logger, "DriverParams: cov_warning=%s", params.cov_warning_ ? "true" : "false");
+    RCLCPP_INFO(logger, "DriverParams: nav2_mode=%s", params.nav2_mode_ ? "true" : "false");
 
-    return true;
+    return ok;
 }
 
-bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node> node, const std::string& ns, CustomerInputParams& params) {
-    const std::string SPEED_TOPIC = ns + ".speed_topic";
-    node->declare_parameter(SPEED_TOPIC, "/fixposition/speed");
-    node->get_parameter(SPEED_TOPIC, params.speed_topic);
-    RCLCPP_INFO(node->get_logger(), "%s : %s", SPEED_TOPIC.c_str(), params.speed_topic.c_str());
+// ---------------------------------------------------------------------------------------------------------------------
 
-    const std::string RTCM_TOPIC = ns + ".rtcm_topic";
-    node->declare_parameter(RTCM_TOPIC, "/fixposition/rtcm");
-    node->get_parameter(RTCM_TOPIC, params.rtcm_topic);
-    RCLCPP_INFO(node->get_logger(), "%s : %s", RTCM_TOPIC.c_str(), params.rtcm_topic.c_str());
+bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node>& nh, const std::string& ns, NodeParams& params) {
+    auto logger = nh->get_logger();
+    bool ok = true;
+    RCLCPP_INFO(logger, "nhParams: loading from %s", ns.c_str());
 
-    return true;
-}
+    const std::string OUTPUT_NS = ns + ".output_ns";
+    const std::string SPEED_TOPIC = ns + ".speed_topic";
+    const std::string CORR_TOPIC = ns + ".corr_topic";
+    const std::string QOS_TYPE = ns + ".qos_type";
 
-bool LoadParamsFromRos2(std::shared_ptr<rclcpp::Node> node, FixpositionDriverParams& params) {
-    bool ok = true;
+    nh->declare_parameter(OUTPUT_NS, params.output_ns_);
+    nh->declare_parameter(SPEED_TOPIC, params.speed_topic_);
+    nh->declare_parameter(CORR_TOPIC, params.corr_topic_);
+    nh->declare_parameter(QOS_TYPE, params.qos_type_);
+
+    if (!nh->get_parameter(OUTPUT_NS, params.output_ns_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", OUTPUT_NS.c_str());
+        ok = false;
+    }
+    if (!nh->get_parameter(SPEED_TOPIC, params.speed_topic_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", SPEED_TOPIC.c_str());
+        ok = false;
+    }
+    if (!nh->get_parameter(CORR_TOPIC, params.corr_topic_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", CORR_TOPIC.c_str());
+        ok = false;
+    }
+    if (!nh->get_parameter(QOS_TYPE, params.qos_type_)) {
+        RCLCPP_WARN(logger, "Failed loading %s param", QOS_TYPE.c_str());
+        ok = false;
+    }
 
-    ok &= LoadParamsFromRos2(node, "fp_output", params.fp_output);
-    ok &= LoadParamsFromRos2(node, "customer_input", params.customer_input);
+    RCLCPP_INFO(logger, "NodeParams: output_ns=%s", params.output_ns_.c_str());
+    RCLCPP_INFO(logger, "NodeParams: speed_topic=%s", params.speed_topic_.c_str());
+    RCLCPP_INFO(logger, "NodeParams: corr_topic=%s", params.corr_topic_.c_str());
+    RCLCPP_INFO(logger, "NodeParams: qos_type=%s", params.qos_type_.c_str());
 
     return ok;
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_odometry_converter_ros1/CMakeLists.txt b/fixposition_odometry_converter_ros1/CMakeLists.txt
index 7df0ca9..85d84e8 100644
--- a/fixposition_odometry_converter_ros1/CMakeLists.txt
+++ b/fixposition_odometry_converter_ros1/CMakeLists.txt
@@ -15,7 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   nav_msgs
   geometry_msgs
-  fixposition_driver_ros1
+  fixposition_driver_msgs
 )
 
 catkin_package(
diff --git a/fixposition_odometry_converter_ros1/README.md b/fixposition_odometry_converter_ros1/README.md
index 83e2acb..029544d 100644
--- a/fixposition_odometry_converter_ros1/README.md
+++ b/fixposition_odometry_converter_ros1/README.md
@@ -3,14 +3,25 @@
 
 ## Description
 
-An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. You can choose to send just the x component of this velocity, both the x and y components, or the entire velocity. If sending more than one component (and intending for it to be used in the fusion engine), make sure that the wheelspeed sensor's configuration has the dimensions field correctly configured.
-The speed values from the selected topic are extracted, converted, and republished to  the `/fixposition/speed` topic, where they will be consumed by the main ROS driver node, and sent to the VRTK2.
-
-_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one value, the total vehicle speed or the total vehicle speed, configured to be RC. For situations where more sensor inputs are desired, a custom converter is necessary._
+An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to
+be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the
+moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted.
+To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. You can choose to send just the x
+component of this velocity, both the x and y components, or the entire velocity. If sending more than one component (and
+intending for it to be used in the fusion engine), make sure that the wheelspeed sensor's configuration has the
+dimensions field correctly configured. The speed values from the selected topic are extracted, converted, and
+republished to  the `/fixposition/speed` topic, where they will be consumed by the main ROS driver node, and sent to the
+VRTK2.
+
+_Please note that currently, the odometry converter only works for situations where the desired input odometry has just
+one value, the total vehicle speed or the total vehicle speed, configured to be RC. For situations where more sensor
+inputs are desired, a custom converter is necessary._
 
 ## Input Parameters
 
-The `odom_converter.yaml` file exposes the necessary parameters for the correct operation of the node. The parameter that may cause the most doubt is the `multiplicative_factor`. This should be chosen such that the inputed float velocity value is transformed into milimeters per second, e.g. 1000 for an input that is expressed in meters per second.
+The `odom_converter.yaml` file exposes the necessary parameters for the correct operation of the node. The parameter
+that may cause the most doubt is the `multiplicative_factor`. This should be chosen such that the inputed float velocity
+value is transformed into milimeters per second, e.g. 1000 for an input that is expressed in meters per second.
 
 ## Launch
 
diff --git a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp
index 27fee2d..33cd03b 100644
--- a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp
+++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp
@@ -1,25 +1,29 @@
 /**
- *  @file
- *  @brief Declaration of OdomConverter class
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Declaration of OdomConverter class
  */
 
-#ifndef __ODOM_CONVERTER_HPP__
-#define __ODOM_CONVERTER_HPP__
+#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ODOM_CONVERTER_HPP__
+#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ODOM_CONVERTER_HPP__
+
+/* LIBC/STL */
 
 /* EXTERNAL */
-#include <fixposition_odometry_converter_ros1/params.hpp>
-#include <fixposition_odometry_converter_ros1/ros_msgs.hpp>
+
+/* PACKAGE */
+#include "fixposition_odometry_converter_ros1/params.hpp"
+#include "fixposition_odometry_converter_ros1/ros_msgs.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 class OdomConverter {
    public:
@@ -77,5 +81,7 @@ class OdomConverter {
 
     OdomInputParams params_;
 };
+
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-#endif  //__FIXPOSITION_DRIVER_FIXPOSITION_DRIVER__
+#endif  //__FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ODOM_CONVERTER_HPP__
diff --git a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp
index dda2432..9d74030 100644
--- a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp
+++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp
@@ -1,24 +1,28 @@
 /**
- *  @file
- *  @brief Parameters for the odometry converter
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Parameters for the odometry converter
  */
 
-#ifndef __ODOM_CONVERTER_PARAMS_HPP__
-#define __ODOM_CONVERTER_PARAMS_HPP__
+#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_PARAMS_HPP__
+#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_PARAMS_HPP__
 
-/* SYSTEM / STL */
+/* LIBC/STL */
 #include <string>
 
+/* EXTERNAL */
+
+/* PACKAGE */
+
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 enum class VelTopicType : int8_t { Twist = 0, TwistWithCov = 1, Odometry = 2 };
 
@@ -40,6 +44,6 @@ struct OdomInputParams {
     bool LoadFromRos(const std::string& ns);
 };
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-
-#endif
+#endif  // __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_PARAMS_HPP__
diff --git a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp
index b5760d5..7061a37 100644
--- a/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp
+++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/ros_msgs.hpp
@@ -1,24 +1,21 @@
 // Wrapper to include ROS stuff without quoting the same warning suppressions all over
-#ifndef __ROS1_DRIVER_CONVERTER_MSGS_HPP__
-#define __ROS1_DRIVER_CONVERTER_MSGS_HPP__
+#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ROS_MSGS_HPP__
+#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ROS_MSGS_HPP__
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #pragma GCC diagnostic ignored "-Wunused-parameter"
 #pragma GCC diagnostic ignored "-Wshadow"
 
-#include <ros/ros.h>
-#include <ros/console.h>
-
-#include <nav_msgs/Odometry.h>
-
-#include <geometry_msgs/Vector3.h>
-#include <geometry_msgs/Vector3Stamped.h>
+#include <fixposition_driver_msgs/Speed.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/TwistWithCovariance.h>
 #include <geometry_msgs/TwistWithCovarianceStamped.h>
-
-#include <fixposition_driver_ros1/Speed.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/console.h>
+#include <ros/ros.h>
 
 #pragma GCC diagnostic pop
-#endif  // __ROS1_DRIVER_CONVERTER_MSGS_HPP__
+#endif  // __FIXPOSITION_ODOMETRY_CONVERTER_ROS1_ROS_MSGS_HPP__
diff --git a/fixposition_odometry_converter_ros1/package.xml b/fixposition_odometry_converter_ros1/package.xml
index f127506..7799c11 100644
--- a/fixposition_odometry_converter_ros1/package.xml
+++ b/fixposition_odometry_converter_ros1/package.xml
@@ -2,15 +2,13 @@
 <package format="2">
     <name>fixposition_odometry_converter_ros1</name>
     <version>1.0.0</version>
-    <description>Conversion from Odometry messages to Fixposition's Speed input</description>
-
-    <maintainer email="info@fixposition.com">Fixposition AG</maintainer>
-
+    <description>Conversion from odometry messages to Fixposition's speed input</description>
+    <maintainer email="support@fixposition.com">Fixposition Support</maintainer>
     <license>MIT</license>
-
     <buildtool_depend>catkin</buildtool_depend>
     <depend>roscpp</depend>
     <depend>geometry_msgs</depend>
     <depend>nav_msgs</depend>
-    <depend>fixposition_driver_ros1</depend>
+    <depend>fixposition_driver_lib</depend>
+    <depend>fixposition_driver_msgs</depend>
 </package>
diff --git a/fixposition_odometry_converter_ros1/src/odom_converter.cpp b/fixposition_odometry_converter_ros1/src/odom_converter.cpp
index d50b7f1..a04e6f3 100644
--- a/fixposition_odometry_converter_ros1/src/odom_converter.cpp
+++ b/fixposition_odometry_converter_ros1/src/odom_converter.cpp
@@ -1,21 +1,26 @@
 /**
- *  @file
- *  @brief Implementation of OdomConverter class
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Implementation of OdomConverter class
  */
 
+/* LIBC/STL */
+
+/* EXTERNAL */
+
 /* PACKAGE */
-#include <fixposition_odometry_converter_ros1/odom_converter.hpp>
+#include "fixposition_odometry_converter_ros1/odom_converter.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
+
 OdomConverter::OdomConverter(ros::NodeHandle* nh) : nh_(*nh) {
     // read parameters
     if (!params_.LoadFromRos("/fixposition_odometry_converter_ros1")) {
@@ -27,7 +32,7 @@ OdomConverter::OdomConverter(ros::NodeHandle* nh) : nh_(*nh) {
     Subscribe();
 
     // initialize the publisher
-    ws_pub_ = nh_.advertise<fixposition_driver_ros1::Speed>(params_.fixposition_speed_topic, 1);
+    ws_pub_ = nh_.advertise<fixposition_driver_msgs::Speed>(params_.fixposition_speed_topic, 1);
 }
 
 void OdomConverter::Subscribe() {
@@ -51,8 +56,8 @@ void OdomConverter::ConvertAndPublish(const std::vector<std::pair<bool, double>>
             ROS_ERROR("Speed vector has an invalid size!");
             return;
         }
-        fixposition_driver_ros1::Speed msg;
-        fixposition_driver_ros1::WheelSensor sensor;
+        fixposition_driver_msgs::Speed msg;
+        fixposition_driver_msgs::WheelSensor sensor;
         sensor.location = "RC";
         sensor.vx_valid = speeds[0].first;
         sensor.vx = round(speeds[0].second * params_.multiplicative_factor);
@@ -89,4 +94,5 @@ void OdomConverter::TwistCallback(const geometry_msgs::TwistConstPtr& msg) {
     ConvertAndPublish(velocities);
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp b/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp
index b8385fc..9e93440 100644
--- a/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp
+++ b/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp
@@ -1,19 +1,22 @@
 /**
- *  @file
- *  @brief Main function for the odometry converter ros node
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Main function for the odometry converter ros node
  */
 
+/* LIBC/STL */
+
+/* EXTERNAL */
+
 /* PACKAGE */
-#include <fixposition_odometry_converter_ros1/odom_converter.hpp>
+#include "fixposition_odometry_converter_ros1/odom_converter.hpp"
 
 int main(int argc, char** argv) {
     ros::init(argc, argv, "fixposition_odometry_converter_ros1");
diff --git a/fixposition_odometry_converter_ros1/src/params.cpp b/fixposition_odometry_converter_ros1/src/params.cpp
index 2b6665d..f5fff92 100644
--- a/fixposition_odometry_converter_ros1/src/params.cpp
+++ b/fixposition_odometry_converter_ros1/src/params.cpp
@@ -1,22 +1,27 @@
 /**
- *  @file
- *  @brief Implementation of Parameter Loading for the odometry parameters
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Implementation of Parameter Loading for the odometry parameters
  */
 
+/* LIBC/STL */
+
+/* EXTERNAL */
+
 /* PACKAGE */
-#include <fixposition_odometry_converter_ros1/params.hpp>
-#include <fixposition_odometry_converter_ros1/ros_msgs.hpp>
+#include "fixposition_odometry_converter_ros1/params.hpp"
+
+#include "fixposition_odometry_converter_ros1/ros_msgs.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 bool OdomInputParams::LoadFromRos(const std::string& ns) {
     if (!ros::param::get(ns + "/fixposition_speed_topic", fixposition_speed_topic))
@@ -48,4 +53,5 @@ bool OdomInputParams::LoadFromRos(const std::string& ns) {
     return true;
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/fixposition_odometry_converter_ros2/CMakeLists.txt b/fixposition_odometry_converter_ros2/CMakeLists.txt
index beaaf2a..636f240 100644
--- a/fixposition_odometry_converter_ros2/CMakeLists.txt
+++ b/fixposition_odometry_converter_ros2/CMakeLists.txt
@@ -14,7 +14,9 @@ set(PACKAGE_INCLUDE_DEPENDS
   rclcpp_components
   nav_msgs
   geometry_msgs
-  fixposition_driver_ros2
+  fpsdk_common
+  fixposition_driver_lib
+  fixposition_driver_msgs
 )
 
 find_package(ament_cmake REQUIRED)
diff --git a/fixposition_odometry_converter_ros2/README.md b/fixposition_odometry_converter_ros2/README.md
index f836182..1cc7339 100644
--- a/fixposition_odometry_converter_ros2/README.md
+++ b/fixposition_odometry_converter_ros2/README.md
@@ -3,14 +3,25 @@
 
 ## Description
 
-An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. You can choose to send just the x component of this velocity, both the x and y components, or the entire velocity. If sending more than one component (and intending for it to be used in the fusion engine), make sure that the wheelspeed sensor's configuration has the `Dimensions` field correctly configured.
-The speed values from the selected topic are extracted, converted, and republished to  the `/fixposition/speed` topic, where they will be consumed by the main ROS driver node, and sent to the VRTK2.
-
-_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one value, the total vehicle speed or the total vehicle speed, configured to be RC. For situations where more sensor inputs are desired, a custom converter is necessary._
+An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to
+be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the
+moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted.
+To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. You can choose to send just the x
+component of this velocity, both the x and y components, or the entire velocity. If sending more than one component (and
+intending for it to be used in the fusion engine), make sure that the wheelspeed sensor's configuration has the
+`Dimensions` field correctly configured. The speed values from the selected topic are extracted, converted, and
+republished to  the `/fixposition/speed` topic, where they will be consumed by the main ROS driver node, and sent to the
+VRTK2.
+
+_Please note that currently, the odometry converter only works for situations where the desired input odometry has just
+one value, the total vehicle speed or the total vehicle speed, configured to be RC. For situations where more sensor
+inputs are desired, a custom converter is necessary._
 
 ## Input Parameters
 
-The `odom_converter.yaml` file exposes the necessary parameters for the correct operation of the node. The parameter that may cause the most doubt is the `multiplicative_factor`. This should be chosen such that the inputed float velocity value is transformed into milimeters per second, e.g. 1000 for an input that is expressed in meters per second.
+The `odom_converter.yaml` file exposes the necessary parameters for the correct operation of the node. The parameter
+that may cause the most doubt is the `multiplicative_factor`. This should be chosen such that the inputed float velocity
+value is transformed into milimeters per second, e.g. 1000 for an input that is expressed in meters per second.
 
 ## Launch
 
diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp
index a879724..808351e 100644
--- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp
+++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp
@@ -1,26 +1,28 @@
 /**
- *  @file
- *  @brief Declaration of OdomConverterNode class
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- *
- * Port to ROS 2 by Husarion
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Declaration of OdomConverterNode class
  */
 
-#ifndef __ODOM_CONVERTER_NODE_HPP__
-#define __ODOM_CONVERTER_NODE_HPP__
+#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ODOM_CONVERTER_HPP__
+#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ODOM_CONVERTER_HPP__
+
+/* LIBC/STL */
+
+/* EXTERNAL */
 
 /* PACKAGE */
-#include <fixposition_odometry_converter_ros2/params.hpp>
+#include "fixposition_odometry_converter_ros2/params.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 class OdomConverterNode : public rclcpp::Node {
    public:
@@ -74,7 +76,9 @@ class OdomConverterNode : public rclcpp::Node {
 
     OdomInputParams params_;
     rclcpp::SubscriptionBase::SharedPtr ws_sub_;
-    rclcpp::Publisher<fixposition_driver_ros2::msg::Speed>::SharedPtr ws_pub_;
+    rclcpp::Publisher<fixposition_driver_msgs::msg::Speed>::SharedPtr ws_pub_;
 };
+
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-#endif  //__ODOM_CONVERTER_NODE_HPP__
+#endif  //__FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ODOM_CONVERTER_HPP__
diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp
index 181571d..fbafe24 100644
--- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp
+++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp
@@ -1,29 +1,29 @@
 /**
- *  @file
- *  @brief Parameters for the odometry converter
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * 
- * Port to ROS 2 by Husarion
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Parameters for the odometry converter
  */
 
-#ifndef __ODOM_CONVERTER_PARAMS_HPP__
-#define __ODOM_CONVERTER_PARAMS_HPP__
+#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_PARAMS_HPP__
+#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_PARAMS_HPP__
 
-/* SYSTEM / STL */
+/* LIBC/STL */
 #include <string>
 
+/* EXTERNAL */
+
 /* PACKAGE */
-#include <fixposition_odometry_converter_ros2/ros2_msgs.hpp>
+#include "fixposition_odometry_converter_ros2/ros2_msgs.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 enum class VelTopicType : int8_t { Twist = 0, TwistWithCov = 1, Odometry = 2 };
 
@@ -47,6 +47,6 @@ struct OdomInputParams {
                      const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr& logging_itf);
 };
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
-
-#endif
+#endif  // __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_PARAMS_HPP__
diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp
index 0d0cc97..2be30c7 100644
--- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp
+++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/ros2_msgs.hpp
@@ -1,19 +1,16 @@
 // Wrapper to include ROS2 stuff without quoting the same warning suppressions all over
-#ifndef __ROS2_DRIVER_CONVERTER_MSGS_HPP__
-#define __ROS2_DRIVER_CONVERTER_MSGS_HPP__
+#ifndef __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ROS2_MSGS_HPP__
+#define __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ROS2_MSGS_HPP__
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #pragma GCC diagnostic ignored "-Wunused-parameter"
 #pragma GCC diagnostic ignored "-Wshadow"
 
-#include <rclcpp/rclcpp.hpp>
-
-#include <nav_msgs/msg/odometry.hpp>
-
+#include <fixposition_driver_msgs/msg/speed.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
-
-#include <fixposition_driver_ros2/msg/speed.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 #pragma GCC diagnostic pop
-#endif  // __ROS2_DRIVER_CONVERTER_MSGS_HPP__
+#endif  // __FIXPOSITION_ODOMETRY_CONVERTER_ROS2_ROS2_MSGS_HPP__
diff --git a/fixposition_odometry_converter_ros2/package.xml b/fixposition_odometry_converter_ros2/package.xml
index 53dec30..84156f8 100644
--- a/fixposition_odometry_converter_ros2/package.xml
+++ b/fixposition_odometry_converter_ros2/package.xml
@@ -1,19 +1,17 @@
 <?xml version="1.0"?>
 <package format="3">
-  <name>fixposition_odometry_converter_ros2</name>
-  <version>1.0.0</version>
-  <description>Conversion from Odometry messages to Fixposition's Speed input for ROS 2</description>
-  <maintainer email="info@fixposition.com">Fixposition AG</maintainer>
-  <license>MIT</license>
-  <author email="maciej.stepien@husarion.com">Maciej Stępień</author>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <exec_depend>rclcpp</exec_depend>
-  <depend>geometry_msgs</depend>
-  <depend>nav_msgs</depend>
-  <depend>fixposition_driver_ros2</depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
+    <name>fixposition_odometry_converter_ros2</name>
+    <version>1.0.0</version>
+    <description>Conversion from Odometry messages to Fixposition's Speed input for ROS 2</description>
+    <maintainer email="support@fixposition.com">Fixposition Support</maintainer>
+    <license>MIT</license>
+    <buildtool_depend>ament_cmake</buildtool_depend>
+    <exec_depend>rclcpp</exec_depend>
+    <depend>fixposition_driver_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>nav_msgs</depend>
+    <depend>fixposition_driver_ros2</depend>
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
 </package>
diff --git a/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp b/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp
index 21214a3..e529a7b 100644
--- a/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp
+++ b/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp
@@ -1,23 +1,25 @@
 /**
- *  @file
- *  @brief Implementation of OdomConverterNode class
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- *
- * Port to ROS 2 by Husarion
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Implementation of OdomConverterNode class
  */
 
+/* LIBC/STL */
+
+/* EXTERNAL */
+
 /* PACKAGE */
-#include <fixposition_odometry_converter_ros2/odom_converter_node.hpp>
+#include "fixposition_odometry_converter_ros2/odom_converter_node.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 OdomConverterNode::OdomConverterNode(const rclcpp::NodeOptions& options) : Node("odom_converter", options) {
     // read parameters
@@ -30,7 +32,7 @@ OdomConverterNode::OdomConverterNode(const rclcpp::NodeOptions& options) : Node(
     Subscribe();
 
     // initialize the publisher
-    ws_pub_ = this->create_publisher<fixposition_driver_ros2::msg::Speed>(params_.fixposition_speed_topic, 10);
+    ws_pub_ = this->create_publisher<fixposition_driver_msgs::msg::Speed>(params_.fixposition_speed_topic, 10);
 }
 
 void OdomConverterNode::Subscribe() {
@@ -57,8 +59,8 @@ void OdomConverterNode::ConvertAndPublish(const std::vector<std::pair<bool, doub
             RCLCPP_ERROR(this->get_logger(), "Speed vector has an invalid size");
             return;
         }
-        fixposition_driver_ros2::msg::Speed msg;
-        fixposition_driver_ros2::msg::WheelSensor sensor;
+        fixposition_driver_msgs::msg::Speed msg;
+        fixposition_driver_msgs::msg::WheelSensor sensor;
         sensor.location = "RC";
         sensor.vx_valid = speeds[0].first;
         sensor.vx = round(speeds[0].second * params_.multiplicative_factor);
@@ -95,8 +97,9 @@ void OdomConverterNode::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr
     ConvertAndPublish(velocities);
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
 
 // Register the component with class_loader
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(fixposition::OdomConverterNode)
\ No newline at end of file
+RCLCPP_COMPONENTS_REGISTER_NODE(fixposition::OdomConverterNode)
diff --git a/fixposition_odometry_converter_ros2/src/params.cpp b/fixposition_odometry_converter_ros2/src/params.cpp
index 709e98b..0d5350d 100644
--- a/fixposition_odometry_converter_ros2/src/params.cpp
+++ b/fixposition_odometry_converter_ros2/src/params.cpp
@@ -1,23 +1,25 @@
 /**
- *  @file
- *  @brief Implementation of Parameter Loading for the odometry parameters
- *
  * \verbatim
- *  ___    ___
- *  \  \  /  /
- *   \  \/  /   Fixposition AG
- *   /  /\  \   All right reserved.
- *  /__/  \__\
- * 
- * Port to ROS 2 by Husarion
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
  * \endverbatim
  *
+ * @file
+ * @brief Implementation of Parameter Loading for the odometry parameters
  */
 
+/* LIBC/STL */
+
+/* EXTERNAL */
+
 /* PACKAGE */
-#include <fixposition_odometry_converter_ros2/params.hpp>
+#include "fixposition_odometry_converter_ros2/params.hpp"
 
 namespace fixposition {
+/* ****************************************************************************************************************** */
 
 bool OdomInputParams::LoadFromRos(const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& param_itf,
                                   const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr& logging_itf) {
@@ -67,4 +69,5 @@ bool OdomInputParams::LoadFromRos(const rclcpp::node_interfaces::NodeParametersI
     return true;
 }
 
+/* ****************************************************************************************************************** */
 }  // namespace fixposition
diff --git a/precommit.sh b/precommit.sh
new file mode 100755
index 0000000..9c0db5e
--- /dev/null
+++ b/precommit.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+set -eEu
+set -o pipefail
+set -o errtrace
+
+SCRIPTDIR=$(dirname $(readlink -f $0))
+cd ${SCRIPTDIR}
+pre-commit run --all-files --hook-stage manual