More documentation, more examples (#70)

examples/fpb_measurements/.gitignore

@@ -0,0 +1 @@
+/build

examples/fpb_measurements/CMakeLists.txt

@@ -0,0 +1,40 @@
+# GENERAL ==============================================================================================================
+
+cmake_minimum_required(VERSION 3.16)
+
+project(fpb_measurements
+    LANGUAGES CXX
+    VERSION 0.0.0
+    DESCRIPTION "Fixposition SDK example: aking a FP_B-MEASUREMENTS message"
+)
+
+set(BUILD_TESTING OFF)
+
+
+# COMPILER SETUP =======================================================================================================
+
+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 Debug)
+endif()
+if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
+    add_compile_definitions(NDEBUG)
+endif()
+
+
+# DEPENDENCIES =========================================================================================================
+
+add_subdirectory(../../fpsdk_common fpsdk_common)
+
+
+# EXECUTABLES ==========================================================================================================
+
+add_executable(${PROJECT_NAME} fpb_measurements.cpp)
+target_link_libraries(${PROJECT_NAME} fpsdk_common)
+
+
+# ======================================================================================================================

examples/fpb_measurements/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+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
+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.

examples/fpb_measurements/README.md

@@ -0,0 +1,3 @@
+# Fixposition SDK example: making a FP_B-MEASUREMENTS message
+
+See the [fpb_measurements.cpp](fpb_measurements.cpp) source code for details.

examples/fpb_measurements/fpb_measurements.cpp

@@ -0,0 +1,165 @@
+/**
+ * \verbatim
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
+ * \endverbatim
+ *
+ * @file
+ * @brief Fixposition SDK examples: aking a FP_B-MEASUREMENTS message
+ *
+ * To build and run:
+ *
+ *     cmake -B build -S .
+ *     cmake --build build --parallel 8
+ *     ./build/fpb_measurements
+ *
+ * This program shows how a FP_B-MEASUREMENTS message can be made.
+ *
+ * This file is both the source code of this example as well as the documentation on how it works.
+ */
+
+/* LIBC/STL */
+#include <cinttypes>
+#include <cmath>
+#include <cstdint>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+
+/* EXTERNAL */
+#include <unistd.h>
+
+/* Fixposition SDK */
+#include <fpsdk_common/app.hpp>
+#include <fpsdk_common/logging.hpp>
+#include <fpsdk_common/parser/fpb.hpp>
+#include <fpsdk_common/types.hpp>
+
+/* PACKAGE */
+
+/* ****************************************************************************************************************** */
+
+using namespace fpsdk::common::time;
+using namespace fpsdk::common::app;
+using namespace fpsdk::common::parser::fpb;
+using namespace fpsdk::common::logging;
+using namespace fpsdk::common::types;
+
+// ---------------------------------------------------------------------------------------------------------------------
+
+int main(int /*argc*/, char** /*argv*/)
+{
+#ifndef NDEBUG
+    fpsdk::common::app::StacktraceHelper stacktrace;
+#endif
+    LoggingSetParams({ LoggingLevel::TRACE });
+
+    // The speed measurement in XYZ in [m/s], two wheels
+    const double speed_FL[3] = { 5.422, -0.02, 0.4 };
+    const double speed_FR[3] = { 6.543, +0.03, 0.5 };
+
+    // The FP_B-MEASUREMENTS message payload consists of two parts:
+    // - 1. The header
+    FpbMeasurementsHead head;
+    head.num_meas = 2;
+    // - 2. The measurements. In this example we have two measurements:
+    FpbMeasurementsMeas meas[2];
+    // - FL wheel
+    meas[0].meas_x = std::floor(speed_FL[0] * 1e3);  // [m/s] -> [mm/s]
+    meas[0].meas_y = std::floor(speed_FL[1] * 1e3);  // [m/s] -> [mm/s]
+    meas[0].meas_z = std::floor(speed_FL[2] * 1e3);  // [m/s] -> [mm/s]
+    meas[0].meas_x_valid = 1;
+    meas[0].meas_y_valid = 1;
+    meas[0].meas_z_valid = 1;
+    meas[0].meas_type = EnumToVal(FpbMeasurementsMeasType::VELOCITY);
+    meas[0].meas_loc = EnumToVal(FpbMeasurementsMeasLoc::FL);
+    meas[0].timestamp_type = EnumToVal(FpbMeasurementsTimestampType::TIMEOFARRIVAL);
+    // - FR wheel
+    meas[1].meas_x = std::floor(speed_FR[0] * 1e3);  // [m/s] -> [mm/s]
+    meas[1].meas_y = std::floor(speed_FR[1] * 1e3);  // [m/s] -> [mm/s]
+    meas[1].meas_z = std::floor(speed_FR[2] * 1e3);  // [m/s] -> [mm/s]
+    meas[1].meas_x_valid = 1;
+    meas[1].meas_y_valid = 1;
+    meas[1].meas_z_valid = 1;
+    meas[1].meas_type = EnumToVal(FpbMeasurementsMeasType::VELOCITY);
+    meas[1].meas_loc = EnumToVal(FpbMeasurementsMeasLoc::FR);
+    meas[1].timestamp_type = EnumToVal(FpbMeasurementsTimestampType::TIMEOFARRIVAL);
+
+    // Create the raw payload by copying the structs above to the right place.
+    uint8_t payload[FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_MAX_NUM_MEAS * FP_B_MEASUREMENTS_MEAS_SIZE)];
+    std::size_t payload_size = 0;
+    // - The header
+    std::memcpy(&payload[payload_size], &head, sizeof(head));
+    payload_size += sizeof(head);
+    // - The data
+    for (std::size_t ix = 0; (ix < head.num_meas) && (ix < FP_B_MEASUREMENTS_MAX_NUM_MEAS); ix++) {
+        std::memcpy(&payload[payload_size], &meas[ix], sizeof(meas[ix]));
+        payload_size += sizeof(meas[ix]);
+    }
+
+    INFO("num_meas=%" PRIu8 " payload_size=%" PRIuMAX, head.num_meas, payload_size);
+    DEBUG_HEXDUMP(payload, payload_size, NULL, NULL);
+
+    // Create FP_B-MEASUREMENTS message
+    std::vector<uint8_t> message;
+    if (FpbMakeMessage(message, FP_B_MEASUREMENTS_MSGID, 0, payload, payload_size)) {
+        INFO("Message successfully made");
+        DEBUG_HEXDUMP(message.data(), message.size(), NULL, NULL);
+
+        // Print to stdio unless that's a terminal
+        if (isatty(fileno(stdout)) == 0) {
+            write(fileno(stdout), message.data(), message.size());
+        }
+    } else {
+        WARNING("Failed making FP_B-MEASUREMENTS message");
+    }
+
+    // clang-format off
+    // This should output:
+    //
+    //     num_meas=2 payload_size=64
+    //     0x0000 00000  01 02 00 00 00 00 00 00  2e 15 00 00 ec ff ff ff  |................|
+    //     0x0010 00016  90 01 00 00 01 01 01 01  03 00 00 00 00 01 00 00  |................|
+    //     0x0020 00032  00 00 00 00 8f 19 00 00  1e 00 00 00 f4 01 00 00  |................|
+    //     0x0030 00048  01 01 01 01 02 00 00 00  00 01 00 00 00 00 00 00  |................|
+    //     Message successfully made
+    //     0x0000 00000  66 21 d1 07 40 00 00 00  01 02 00 00 00 00 00 00  |f!..@...........|
+    //     0x0010 00016  2e 15 00 00 ec ff ff ff  90 01 00 00 01 01 01 01  |................|
+    //     0x0020 00032  03 00 00 00 00 01 00 00  00 00 00 00 8f 19 00 00  |................|
+    //     0x0030 00048  1e 00 00 00 f4 01 00 00  01 01 01 01 02 00 00 00  |................|
+    //     0x0040 00064  00 01 00 00 00 00 00 00  22 59 21 e2
+    //
+    // The message can be stored to a file:
+    //
+    //     ./build/fpb_measurements > fpbmeasurements.bin
+    //
+    // Which can be parsed by the parsertool from the Fixposition SDK:
+    //
+    //     parsertool fpbmeasurements.bin
+    //
+    // Which prints something like this:
+    //
+    //     ------- Seq#     Offset  Size Protocol Message                        Info
+    //     Reading from fpbmeasurements.bin
+    //     message 000001        0    76 FP_B     FP_B-MEASUREMENTS              07d1@0 v1 [2] / 1 3 1 0 0 5422 -20 400 / 1 2 1 0 0 6543 30 500
+    //     Stats:     Messages               Bytes
+    //     Total             1 (100.0%)         76 (100.0%)
+    //     FP_A              0 (  0.0%)          0 (  0.0%)
+    //     FP_B              1 (100.0%)         76 (100.0%)
+    //     NMEA              0 (  0.0%)          0 (  0.0%)
+    //     UBX               0 (  0.0%)          0 (  0.0%)
+    //     RTCM3             0 (  0.0%)          0 (  0.0%)
+    //     NOV_B             0 (  0.0%)          0 (  0.0%)
+    //     UNI_B             0 (  0.0%)          0 (  0.0%)
+    //     SPARTN            0 (  0.0%)          0 (  0.0%)
+    //     OTHER             0 (  0.0%)          0 (  0.0%)
+    //     Done
+    // clang-format on
+
+    return EXIT_SUCCESS;
+}
+
+/* ****************************************************************************************************************** */

examples/fusion_epoch/fusion_epoch.cpp

@@ -180,9 +180,9 @@ int main(int /*argc*/, char** /*argv*/)
     //     Using position at 2349:059923.000: [ 4278387.7, 635620.5, 4672339.9 ]                 (4)
     //     Local coordinates: 2676373.0 1250433.7 459.5
     //
-    // - (1) The first epoch is incomplete as the FP_A-ODOMETRY for that epoch was is present
-    // - (2) The second epoch is complete (all messages present and matching), but FP_A-ODOMSTATUS.init_status is 1 (and
-    //       need want 2 in order to use the data)
+    // - (1) The first epoch is incomplete as the FP_A-ODOMETRY for that epoch was not present
+    // - (2) The second epoch is complete (all messages present and matching), but FP_A-ODOMSTATUS.init_status is
+    //       LOCAL_INIT and we want status GLOBAL_INIT in order to use the data)
     // - (3) The third epoch is complete, but the FP_A-ODOMETRY.pos_{x,y,z} fields are empty (and we want the position)
     // - (4) The fourth epoch is complete and passes all checks and we can transform the position to the local CRS
 
@@ -227,6 +227,11 @@ static void ProcessCollectedMsgs(const CollectedMsgs& collected_msgs, Transforme
     if (trafo.Transform(pos)) {
         INFO("Local coordinates: %.1f %.1f %.1f", pos(0), pos(1), pos(2));
     }
+
+    // Notes:
+    // - The same approach can be used to collect NMEA messages into an epoch. For example, using the FP_A-EOE for GNSS,
+    //   this lets one reliably combine all NMEA messages for GNSS, including those that do not have a timestamp, into
+    //   a consistent set of messages that are part of the same naviation epoch.
 }
 
 /* ****************************************************************************************************************** */

fpsdk_common/include/fpsdk_common/parser/fpb.hpp

@@ -261,6 +261,11 @@ static constexpr const char* FP_B_UNITTEST2_STRID           = "FP_B-UNITTEST2";
  *
  * This message is used to input measurements, such as wheelspeeds, to the sensor.
  *
+ * **Notes:**
+ *
+ * - The supported `meas_type`, `meas_loc` and `timestamp_type` values depend on the sensor software and configuration
+ *   used. Refer to the sensor documentation for valid combinations of values for these fields.
+ *
  * **Payload fields:**
  *
  * |         # | Offset    | Field             | Type           | Unit | Description                                                            |